Skip to content

Q
qgroundcontrol
Commit 18128c94 authored by lm's avatar lm
Browse files
Options

Merged

parents cec2d6f5 39eabe5f
Hide whitespace changes
Inline Side-by-side
Showing with 231 additions and 107 deletions
qgroundcontrol.pro
View file @ 18128c94
......@@ -422,9 +422,9 @@ contains(DEPENDENCIES_PRESENT, libfreenect) {
RESOURCES += mavground.qrc
# 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")
LIBS += -LC:\OPAL-RT\RT-LAB7.2.4\Common\bin \
LIBS += -LC:/OPAL-RT/RT-LAB7.2.4/Common/bin \
-lOpalApi
INCLUDEPATH += src/lib/opalrt
HEADERS += src/comm/OpalRT.h \
......
settings/ParameterList.xml
View file @ 18128c94
......@@ -18,7 +18,48 @@
<!--Parameters related to the controller block -->
<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>
<!-- Paremters for the Pilot Input/Raw RC block -->
......@@ -27,7 +68,7 @@
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/"
SimulinkParameterName="Setpoint0"
QGCParamID="AIL_RIGHT_IN"
QGCParamID="AIL_LEFT_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/"
......@@ -37,7 +78,7 @@
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/"
SimulinkParameterName="Setpoint2"
QGCParamID="AIL_LEFT_IN"
QGCParamID="AIL_RIGHT_IN"
/>
<!-- Settings for Elevator Servo -->
<Parameter
......@@ -143,45 +184,44 @@
<!-- Settings for Aileron Servo -->
<Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/AileronOutput/"
SimulinkParameterName="Low"
QGCParamID="AIL_LOW_OUT"
SimulinkParameterName="Setpoint0"
QGCParamID="AIL_LEFT_OUT"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/AileronOutput/"
SimulinkParameterName="Center"
SimulinkParameterName="Setpoint1"
QGCParamID="AIL_CENTER_OUT"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/AileronOutput/"
SimulinkParameterName="High"
QGCParamID="AIL_HIGH_OUT"
SimulinkParameterName="Setpoint2"
QGCParamID="AIL_RIGHT_OUT"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/AileronOutput/"
SimulinkParameterName="Reverse"
QGCParamID="AIL_REV_OUT"
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/AileronMix/Controller_Mix/"
SimulinkParameterName="Constant Value"
QGCParamID="MIX_AIL"
/>
<!-- Settings for Elevator Servo -->
<Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/ElevatorOutput/"
SimulinkParameterName="Low"
QGCParamID="ELE_LOW_OUT"
SimulinkParameterName="Setpoint0"
QGCParamID="ELE_DOWN_OUT"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/ElevatorOutput/"
SimulinkParameterName="Center"
SimulinkParameterName="Setpoint1"
QGCParamID="ELE_CENTER_OUT"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/ElevatorOutput/"
SimulinkParameterName="High"
QGCParamID="ELE_HIGH_OUT"
SimulinkParameterName="Setpoint2"
QGCParamID="ELE_UP_OUT"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/ElevatorOutput/"
SimulinkParameterName="Reverse"
QGCParamID="ELE_REV_OUT"
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/ElevatorMix/Controller_Mix/"
SimulinkParameterName="Constant Value"
QGCParamID="MIX_ELE"
/>
</Block>
</ParameterList>
src/comm/OpalLink.cc
View file @ 18128c94
......@@ -41,7 +41,9 @@ OpalLink::OpalLink() :
componentID(1),
params(NULL),
opalInstID(101),
sendRCValues(false)
sendRCValues(false),
sendRawController(false),
sendPosition(false)
{
start(QThread::LowPriority);
......@@ -134,13 +136,13 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
}
}
break;
case MAVLINK_MSG_ID_REQUEST_RC_CHANNELS:
{
mavlink_request_rc_channels_t rc;
mavlink_msg_request_rc_channels_decode(&msg, &rc);
this->sendRCValues = static_cast<bool>(rc.enabled);
}
break;
// case MAVLINK_MSG_ID_REQUEST_RC_CHANNELS:
// {
// mavlink_request_rc_channels_t rc;
// mavlink_msg_request_rc_channels_decode(&msg, &rc);
// this->sendRCValues = static_cast<bool>(rc.enabled);
// }
// break;
#ifdef MAVLINK_ENABLED_UALBERTA_MESSAGES
case MAVLINK_MSG_ID_RADIO_CALIBRATION:
{
......@@ -157,17 +159,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,11 +219,50 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
}
break;
#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:
{
qDebug() << "OpalLink::writeBytes(): Unknown mavlink packet";
}
}
#endif
}
}
......@@ -282,17 +335,19 @@ void OpalLink::getSignals()
if (returnVal == EOK )
{
/* Send position info to qgroundcontrol */
mavlink_message_t local_position;
mavlink_msg_local_position_pack(systemID, componentID, &local_position,
(*timestep)*1000000,
values[OpalRT::X_POS],
values[OpalRT::Y_POS],
values[OpalRT::Z_POS],
values[OpalRT::X_VEL],
values[OpalRT::Y_VEL],
values[OpalRT::Z_VEL]);
receiveMessage(local_position);
if (sendPosition)
{
mavlink_message_t local_position;
mavlink_msg_local_position_pack(systemID, componentID, &local_position,
(*timestep)*1000000,
values[OpalRT::X_POS],
values[OpalRT::Y_POS],
values[OpalRT::Z_POS],
values[OpalRT::X_VEL],
values[OpalRT::Y_VEL],
values[OpalRT::Z_VEL]);
receiveMessage(local_position);
}
/* send attitude info to qgroundcontrol */
mavlink_message_t attitude;
mavlink_msg_attitude_pack(systemID, componentID, &attitude,
......@@ -336,6 +391,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
{
......@@ -408,10 +475,12 @@ uint8_t OpalLink::rescaleNorm(double norm, int ch)
return static_cast<uint8_t>(norm*255);
break;
}
}
int8_t OpalLink::rescaleControllerOutput(double raw)
{
return static_cast<int8_t>((raw>=0?raw*127:raw*128));
}
bool OpalLink::connect()
{
......
src/comm/OpalLink.h
View file @ 18128c94
......@@ -153,8 +153,11 @@ protected:
uint16_t duty2PulseMicros(double duty);
uint8_t rescaleNorm(double norm, int ch);
int8_t rescaleControllerOutput(double raw);
bool sendRCValues;
bool sendRawController;
bool sendPosition;
};
#endif // OPALLINK_H
src/comm/OpalRT.h
View file @ 18128c94
......@@ -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
};
......
src/comm/Parameter.h
View file @ 18128c94
......@@ -61,8 +61,8 @@ namespace OpalRT
const QGCParamID& getParamID() const {return *paramID;}
void setOpalID(unsigned short opalID) {this->opalID = opalID;}
const QString& getSimulinkPath() {return *simulinkPath;}
const QString& getSimulinkName() {return *simulinkName;}
const QString& getSimulinkPath() const {return *simulinkPath;}
const QString& getSimulinkName() const {return *simulinkName;}
uint8_t getComponentID() const {return componentID;}
float getValue();
void setValue(float value);
......
src/comm/ParameterList.cc
View file @ 18128c94
......@@ -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;
}
}
}
src/comm/ParameterList.h
View file @ 18128c94
......@@ -53,11 +53,11 @@ namespace OpalRT
const_iterator(const const_iterator& other);
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);}
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);}
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];}
......
src/ui/HUD.cc
View file @ 18128c94
......@@ -721,8 +721,8 @@ void HUD::paintHUD()
painter.drawRoundedRect(compassRect, 2, 2);
QString yawAngle;
const float yawDeg = ((values.value("yaw", 0.0f)/M_PI)*180.0f)+180.f;
qDebug() << "YAW: " << yawDeg;
// 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);
yawAngle.sprintf("%03d", (int)yawDeg);
paintText(yawAngle, defaultColor, 3.5f, -3.7f, compassY+ 0.9f, &painter);
......
src/ui/RadioCalibration/RadioCalibrationData.cc
View file @ 18128c94
......@@ -48,18 +48,14 @@ const float* RadioCalibrationData::operator [](int i) const
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];
}
// FIXME Bryan
// FIXME James
// This is not good. If it is ever used after being returned it will cause a crash
// return QVector<float>();
throw std::out_of_range("Invalid channel index");
}
QString RadioCalibrationData::toString(RadioElement element) const
......
src/ui/RadioCalibration/RadioCalibrationData.h
View file @ 18128c94
......@@ -34,6 +34,7 @@ This file is part of the QGROUNDCONTROL project
#include <QDebug>
#include <QVector>
#include <QString>
#include <stdexcept>
/**
......@@ -66,7 +67,7 @@ public:
};
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;}
public slots:
......
src/ui/RadioCalibration/RadioCalibrationWindow.cc
View file @ 18128c94
......@@ -52,38 +52,38 @@ RadioCalibrationWindow::RadioCalibrationWindow(QWidget *parent) :
setUASId(0);
}
void RadioCalibrationWindow::setChannelRaw(int ch, float raw)
{
/** this expects a particular channel to function mapping
\todo allow run-time channel mapping
*/
switch (ch)
{
case 0:
aileron->channelChanged(raw);
break;
case 1:
elevator->channelChanged(raw);
break;
case 2:
throttle->channelChanged(raw);
break;
case 3:
rudder->channelChanged(raw);
break;
case 4:
gyro->channelChanged(raw);
break;
case 5:
pitch->channelChanged(raw);
break;
//void RadioCalibrationWindow::setChannelRaw(int ch, float raw)
//{
// /** this expects a particular channel to function mapping
// \todo allow run-time channel mapping
// */
// switch (ch)
// {
// case 0:
// aileron->channelChanged(raw);
// break;
// case 1:
// elevator->channelChanged(raw);
// break;
// case 2:
// throttle->channelChanged(raw);
// break;
// case 3:
// rudder->channelChanged(raw);
// break;
// case 4:
// gyro->channelChanged(raw);
// break;
// case 5:
// pitch->channelChanged(raw);
// break;
}
}
// }
//}
void RadioCalibrationWindow::setChannelScaled(int ch, float normalized)
{
//void RadioCalibrationWindow::setChannelScaled(int ch, float normalized)
//{
// FIXME James
// FIXME Bryan
......@@ -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
\todo allow run-time channel mapping
......
src/ui/RadioCalibration/RadioCalibrationWindow.h
View file @ 18128c94
......@@ -66,9 +66,10 @@ public:
explicit RadioCalibrationWindow(QWidget *parent = 0);
public slots:
void setChannel(int ch, float raw, float normalized);
void setChannelRaw(int ch, float raw);
void setChannelScaled(int ch, float normalized);
void setChannel(int ch, float raw);
// @todo remove these functions if they are not needed - were added by lm on dec 14, 2010
// void setChannelRaw(int ch, float raw);
// void setChannelScaled(int ch, float normalized);
void loadFile();
void saveFile();
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