To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit bcc1fd5f authored by elokdae's avatar elokdae
Browse files

Sinusoidal thrust + rate perturbation + minor GUI changes

parent a0bdfefa
......@@ -56,7 +56,7 @@
<property name="bottomMargin">
<number>6</number>
</property>
<item row="0" column="3">
<item row="0" column="4">
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
......@@ -108,7 +108,7 @@
</property>
</widget>
</item>
<item row="1" column="2">
<item row="2" column="2">
<layout class="QHBoxLayout" name="horizontalLayout_2">
<property name="leftMargin">
<number>0</number>
......@@ -134,7 +134,7 @@
</item>
</layout>
</item>
<item row="1" column="1">
<item row="2" column="1">
<layout class="QHBoxLayout" name="horizontalLayout_3">
<property name="leftMargin">
<number>0</number>
......@@ -160,7 +160,7 @@
</item>
</layout>
</item>
<item row="1" column="0">
<item row="2" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_4">
<property name="leftMargin">
<number>0</number>
......@@ -186,6 +186,35 @@
</item>
</layout>
</item>
<item row="0" column="3">
<widget class="QPushButton" name="custom_button_4">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Button 4</string>
</property>
</widget>
</item>
<item row="2" column="3">
<widget class="QLineEdit" name="lineEdit_custom_4">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16777215</height>
</size>
</property>
</widget>
</item>
</layout>
</item>
<item row="1" column="0">
......
......@@ -115,6 +115,8 @@ private slots:
void on_custom_button_4_clicked();
private:
Ui::TemplateControllerTab *ui;
......
......@@ -145,8 +145,6 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
ui->action_showHideController_picker->trigger();
// > For the tuning controller
ui->action_showHideController_tuning->trigger();
// > For the template controller
ui->action_showHideController_template->trigger();
}
......
......@@ -178,6 +178,12 @@ void TemplateControllerTab::on_custom_button_3_clicked()
#endif
}
void TemplateControllerTab::on_custom_button_4_clicked()
{
#ifdef CATKIN_MAKE
publish_custom_button_command(4,ui->lineEdit_custom_4);
#endif
}
......
......@@ -167,12 +167,45 @@ std::vector<float> yaml_gainMatrixYawRate = { 0.00, 0.00, 0.00,
float yaml_thrustPerturbAmp_in_grams = 0.0;
float yaml_thrustPerturbFreq = 0.0;
// Roll rate perturbation parameters
float yaml_rollRatePerturbAmp_in_deg = 0.0;
float yaml_rollRatePerturbFreq = 0.0;
// Pitch rate perturbation parameters
float yaml_pitchRatePerturbAmp_in_deg = 0.0;
float yaml_pitchRatePerturbFreq = 0.0;
// Yaw rate perturbation parameters
float yaml_yawRatePerturbAmp_in_deg = 0.0;
float yaml_yawRatePerturbFreq = 0.0;
// The weight of the Crazyflie in Newtons, i.e., mg
float m_cf_weight_in_newtons = 0.0;
// Thrust perturbation in Newtons
float m_thrustPerturbAmp_in_newtons = 0.0;
// Roll rate perturbation in rad/s
float m_rollRatePerturbAmp_in_rad = 0.0;
// Pitch rate perturbation in rad/s
float m_pitchRatePerturbAmp_in_rad = 0.0;
// Yaw rate perturbation in rad/s
float m_yawRatePerturbAmp_in_rad = 0.0;
// Enable thrust perturbation
bool m_thrustPerturbEnable = false;
// Enable roll rate perturbation
bool m_rollRatePerturbEnable = false;
// Enable pitch rate perturbation
bool m_pitchRatePerturbEnable = false;
// Enable yaw rate perturbation
bool m_yawRatePerturbEnable = false;
// The location error of the Crazyflie at the "previous" time step
float m_previous_stateErrorInertial[9];
......
......@@ -24,7 +24,7 @@ mocap_timeout_duration: 2.0
# Flag for whether the take-off should be performed
# with the default controller
shouldPerfom_takeOff_with_defaultController: true
shouldPerfom_takeOff_with_defaultController: false
# Flag for whether the landing should be performed
# with the default controller
......
......@@ -28,7 +28,25 @@ gainMatrixPitchRate : [ 6.20, 0.00, 0.00, 3.00, 0.00, 0.00, 0.0
gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
# Thrust perturbation magnitude, in grams
thrustPerturbAmp : 50
thrustPerturbAmp : 20
# Thrust perturbation frequency, in hertz
thrustPerturbFreq: 0.5
thrustPerturbFreq: 0.1
# Roll rate perturbation magnitude, in deg/s
rollRatePerturbAmp : 60
# Roll rate perturbation frequency, in hertz
rollRatePerturbFreq: 0.1
# Pitch rate perturbation magnitude, in deg/s
pitchRatePerturbAmp : 60
# Pitch rate perturbation frequency, in hertz
pitchRatePerturbFreq: 0.1
# Yaw rate perturbation magnitude, in deg/s
yawRatePerturbAmp : 180
# Yaw rate perturbation frequency, in hertz
yawRatePerturbFreq: 0.1
\ No newline at end of file
......@@ -148,7 +148,7 @@ void requestLoadYamlFilenameCallback(const StringWithHeader& yaml_filename_to_lo
}
}
// Sleep to make the publisher known to ROS (in seconds)
ros::Duration(1.0).sleep();
ros::Duration(2.0).sleep();
// Send the message
yamlParametersReadyForFetchPublisher.publish(yaml_ready_msg);
......
......@@ -257,6 +257,14 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
response.controlOutput.pitch = pitchRate_forResponse;
response.controlOutput.yaw = yawRate_forResponse;
// Roll/pitch/yaw rate perturbation
if (m_rollRatePerturbEnable)
response.controlOutput.roll += m_rollRatePerturbAmp_in_rad * sin(2 * PI * yaml_rollRatePerturbFreq * m_time);
if (m_pitchRatePerturbEnable)
response.controlOutput.pitch += m_pitchRatePerturbAmp_in_rad * sin(2 * PI * yaml_pitchRatePerturbFreq * m_time);
if (m_yawRatePerturbEnable)
response.controlOutput.yaw += m_yawRatePerturbAmp_in_rad * sin(2 * PI * yaml_yawRatePerturbFreq * m_time);
// Put the thrust commands into the "response" variable.
// The thrust adjustment computed by the controller need to be added to the
// the feed-forward thrust that "counter-acts" gravity.
......@@ -267,8 +275,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// as feed-forward. Assuming the the Crazyflie is symmetric, this
// value is divided by four.
float feed_forward_thrust_per_motor = m_cf_weight_in_newtons / 4.0;
float thrust_perturb_per_motor = m_thrustPerturbAmp_in_newtons / 4.0 * sin(2 * PI * yaml_thrustPerturbFreq * m_time);
float thrust_request_per_motor = thrustAdjustment / 4.0 + feed_forward_thrust_per_motor + thrust_perturb_per_motor;
float thrust_request_per_motor = thrustAdjustment / 4.0 + feed_forward_thrust_per_motor;
// Thrust perturbation
if (m_thrustPerturbEnable)
thrust_request_per_motor += m_thrustPerturbAmp_in_newtons / 4.0 * sin(2 * PI * yaml_thrustPerturbFreq * m_time);
// > NOTE: the function "computeMotorPolyBackward" converts the input argument
// from Newtons to the 16-bit command expected by the Crazyflie.
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrust_request_per_motor);
......@@ -605,7 +617,14 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
// Let the user know that this part of the code was triggered
ROS_INFO_STREAM("[TEMPLATE CONTROLLER] Button 1 received in controller, with message.float_data = " << float_data );
// Code here to respond to custom button 1
if (m_thrustPerturbEnable)
m_thrustPerturbEnable = false;
else
{
m_time = 0.0;
m_thrustPerturbEnable = true;
}
break;
}
......@@ -615,6 +634,13 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
// Let the user know that this part of the code was triggered
ROS_INFO_STREAM("[TEMPLATE CONTROLLER] Button 2 received in controller, with message.float_data = " << float_data );
// Code here to respond to custom button 2
if (m_rollRatePerturbEnable)
m_rollRatePerturbEnable = false;
else
{
m_time = 0.0;
m_rollRatePerturbEnable = true;
}
break;
}
......@@ -625,6 +651,30 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
// Let the user know that this part of the code was triggered
ROS_INFO_STREAM("[TEMPLATE CONTROLLER] Button 3 received in controller, with message.float_data = " << float_data );
// Code here to respond to custom button 3
if (m_pitchRatePerturbEnable)
m_pitchRatePerturbEnable = false;
else
{
m_time = 0.0;
m_pitchRatePerturbEnable = true;
}
break;
}
// > FOR CUSTOM BUTTON 4
case 4:
{
// Let the user know that this part of the code was triggered
ROS_INFO_STREAM("[TEMPLATE CONTROLLER] Button 4 received in controller, with message.float_data = " << float_data );
// Code here to respond to custom button 3
if (m_yawRatePerturbEnable)
m_yawRatePerturbEnable = false;
else
{
m_time = 0.0;
m_yawRatePerturbEnable = true;
}
break;
}
......@@ -658,6 +708,36 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
// ----------------------------------------------------------------------------------
// TIMER CALLBACK FOR SENDING THE LOAD YAML REQUEST
// This function does NOT need to be edited
void timerCallback_initial_load_yaml(const ros::TimerEvent&)
{
// Create a node handle to the selected parameter service
ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
// Create the service client as a local variable
ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
// Create the service call as a local variable
LoadYamlFromFilename loadYamlFromFilenameCall;
// Specify the Yaml filename as a string
loadYamlFromFilenameCall.request.stringWithHeader.data = "TemplateController";
// Set for whom this applies to
loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForAgentID = false;
// Wait until the serivce exists
requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
// Make the service call
if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
{
// Nothing to do in this case.
// The "isReadyTemplateControllerYamlCallback" function
// will be called once the YAML file is loaded
}
else
{
// Inform the user
ROS_ERROR("[TEMPLATE CONTROLLER] The request load yaml file service call failed.");
}
}
// CALLBACK NOTIFYING THAT THE YAML PARAMETERS ARE READY TO BE LOADED
void isReadyTemplateControllerYamlCallback(const IntWithHeader & msg)
{
......@@ -752,10 +832,22 @@ void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle)
getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate", yaml_gainMatrixYawRate, 9);
// Thrust perturbtion parameters
// Thrust perturbation parameters
yaml_thrustPerturbAmp_in_grams = getParameterFloat(nodeHandle_for_paramaters, "thrustPerturbAmp");
yaml_thrustPerturbFreq = getParameterFloat(nodeHandle_for_paramaters, "thrustPerturbFreq");
// Roll rate perturbation parameters
yaml_rollRatePerturbAmp_in_deg = getParameterFloat(nodeHandle_for_paramaters, "rollRatePerturbAmp");
yaml_rollRatePerturbFreq = getParameterFloat(nodeHandle_for_paramaters, "rollRatePerturbFreq");
// Pitch rate perturbation parameters
yaml_pitchRatePerturbAmp_in_deg = getParameterFloat(nodeHandle_for_paramaters, "pitchRatePerturbAmp");
yaml_pitchRatePerturbFreq = getParameterFloat(nodeHandle_for_paramaters, "pitchRatePerturbFreq");
// Yaw rate perturbation parameters
yaml_yawRatePerturbAmp_in_deg = getParameterFloat(nodeHandle_for_paramaters, "yawRatePerturbAmp");
yaml_yawRatePerturbFreq = getParameterFloat(nodeHandle_for_paramaters, "yawRatePerturbFreq");
// > DEBUGGING: Print out one of the parameters that was loaded to
// debug if the fetching of parameters worked correctly
ROS_INFO_STREAM("[TEMPLATE CONTROLLER] DEBUGGING: the fetched TemplateController/mass = " << yaml_cf_mass_in_grams);
......@@ -768,9 +860,18 @@ void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle)
// gravity (i.e., mg) in units of [Newtons]
m_cf_weight_in_newtons = yaml_cf_mass_in_grams * 9.81/1000.0;
// > Compute the the thrust perturbtion force in units of [Newtons]
// > Compute the thrust perturbation force in units of [Newtons]
m_thrustPerturbAmp_in_newtons = yaml_thrustPerturbAmp_in_grams * 9.81/1000.0;
// > Compute the roll rate perturbation in units of [rad/s]
m_rollRatePerturbAmp_in_rad = yaml_rollRatePerturbAmp_in_deg * PI/180.0;
// > Compute the pitch rate perturbation in units of [rad/s]
m_pitchRatePerturbAmp_in_rad = yaml_pitchRatePerturbAmp_in_deg * PI/180.0;
// > Compute the yaw rate perturbation in units of [rad/s]
m_yawRatePerturbAmp_in_rad = yaml_yawRatePerturbAmp_in_deg * PI/180.0;
// DEBUGGING: Print out one of the computed quantities
ROS_INFO_STREAM("[TEMPLATE CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << m_cf_weight_in_newtons);
}
......@@ -895,29 +996,9 @@ int main(int argc, char* argv[]) {
// we stall the availability of this node until the
// paramter service is ready
// Create the service client as a local variable
ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
// Create the service call as a local variable
LoadYamlFromFilename loadYamlFromFilenameCall;
// Specify the Yaml filename as a string
loadYamlFromFilenameCall.request.stringWithHeader.data = "TemplateController";
// Set for whom this applies to
loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForAgentID = false;
// Wait until the serivce exists
requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
// Make the service call
if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
{
// Nothing to do in this case.
// The "isReadyTemplateControllerYamlCallback" function
// will be called once the YAML file is loaded
}
else
{
// Inform the user
ROS_ERROR("[TEMPLATE CONTROLLER] The request load yaml file service call failed.");
}
// Create a single-shot timer
ros::Timer timer_initial_load_yaml = nodeHandle.createTimer(ros::Duration(1.0), timerCallback_initial_load_yaml, true);
timer_initial_load_yaml.start();
......
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