Skip to content

Q
qgroundcontrol
Commit 84b4fb48 authored by Bryan Godbolt's avatar Bryan Godbolt
Browse files
Options

Completed code for interface to basic attitude controller

parent d0c837d1
Hide whitespace changes
Inline Side-by-side
Showing with 126 additions and 37 deletions
settings/ParameterList.xml
View file @ 84b4fb48
......@@ -18,7 +18,38 @@
<!--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"
/>
<!-- 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"
/>
</Block>
<!-- Paremters for the Pilot Input/Raw RC block -->
......@@ -27,7 +58,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 +68,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 +174,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 @ 84b4fb48
......@@ -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<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 @ 84b4fb48
......@@ -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
src/comm/OpalRT.h
View file @ 84b4fb48
......@@ -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/ParameterList.cc
View file @ 84b4fb48
......@@ -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;
}
}
}
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