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 db7970ff authored by mastefan's avatar mastefan
Browse files

VM: feedforward finished

parent f8899d57
......@@ -1637,10 +1637,17 @@ void MainWindow::on_set_weigth_param_clicked()
{
Setpoint msg;
if(!ui->weight_param->text().isEmpty()){
msg.x = (ui->weight_param->text()).toFloat();
}
/*
msg.x = (ui->weight_param->text()).toFloat();
msg.y = 0;
msg.z = 0;
*/
this->droneXWeightParamPublisher.publish(msg);
}
......
......@@ -245,8 +245,9 @@ float trajectory_period = 5; // 1 round in 5 sec
float thrust_factor = 1;
float pitchAngle_baseline = 1;
float rollAngle_baseline = 1;
......
......@@ -15,7 +15,7 @@ vicon_frequency : 200
# controller_mode : 0: Basic Controller
# controller_mode : 1: Angle Controller
# controller_mode : 2: Trajectory Controller
controller_mode : 2
controller_mode : 1
......
......@@ -263,12 +263,14 @@ void integratorCallback (const Setpoint& integrParams ) {
void WeightParamCallback (const Setpoint& weightParam ) {
// TODO for changing yaml: set weight in yaml OR just set m_mass_CF_grams?
// m_mass_CF_grams = weightParam.x;
m_mass_total_grams = weightParam.x;
}
// Add a factor to the Pitchbaseline (default factor is 1)
void PitchBaselineParamCallback(const Setpoint& pitchAngleParam ) {
// TODO
pitchAngle_baseline = pitchAngleParam.x;
}
......@@ -393,7 +395,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
m_time_seconds = float(m_time_ticks) / m_vicon_frequency;
ROS_INFO_STREAM("Mass of crazyflie " << m_mass_total_grams );
switch(flying_state){
case DRONEX_STATE_APPROACH:
......@@ -522,11 +524,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
{
//ROS_INFO_STREAM("DRONEX_STATE_HOVER");
// keep setpoint constant
/*
dronexSetpoint.x = dronexSetpoint.x;
dronexSetpoint.y = dronexSetpoint.y;
dronexSetpoint.z = dronexSetpoint.z;
*/
dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z+0.3;
}
break;
......@@ -774,30 +777,35 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
}
// Calculate the angle in x-y plane of DroneX for transformation to local coordinates
float droneXAngle;
float ms_velocity_norm = sqrt(pow(ms_velocity[0], 2.0) + pow(ms_velocity[1], 2.0)); //(ms_velocity[0])^2 * (ms_velocity[1])^2);
if(ms_velocity_norm != 0){
if (ms_velocity[1] >= 0)
{
droneXAngle = acos(ms_velocity[0] / ms_velocity_norm);
}else{
droneXAngle = -acos(ms_velocity[0] / ms_velocity_norm);
}
}
// if(ms_velocity_norm != 0){
// if (ms_velocity[1] >= 0)
// {
// droneXAngle = acos(ms_velocity[0] / ms_velocity_norm);
// }else{
// droneXAngle = -acos(ms_velocity[0] / ms_velocity_norm);
// }
// }
// Calculcate Roll and Pitch Baseline, which comes from the moving mothership
float rollAngle_baseline = 1;
float pitchAngle_baseline = 1;
float sinYaw = sin(request.ownCrazyflie.yaw);
float cosYaw = cos(request.ownCrazyflie.yaw);
// TODO
rollAngle_forResponse += rollAngle_baseline * (cos(droneXAngle) * ms_velocity[0]- sin(droneXAngle) * ms_velocity[1]);
pitchAngle_forResponse += pitchAngle_forResponse * (sin(droneXAngle) * ms_velocity[0] + cos(droneXAngle) * ms_velocity[1]);
// Fill in the (x,y,z) position estimates to be returned
float cf_vel_baseline_body_x = ms_velocity[0] * cosYaw + ms_velocity[1] * sinYaw;
float cf_vel_baseline_body_y = ms_velocity[1] * cosYaw - ms_velocity[0] * sinYaw;
// Calculcate Roll and Pitch Baseline, which comes from the moving mothership
rollAngle_forResponse -= (20.0 * DEG2RAD) * cf_vel_baseline_body_y;
pitchAngle_forResponse += (20.0 * DEG2RAD) * cf_vel_baseline_body_x;
// Calculate the Force Feedforward
float F_in_newtons = (gravity * m_mass_total_grams)/(cos(rollAngle_forResponse)*cos(pitchAngle_forResponse)*1000.0);
float F_in_newtons = (gravity * m_mass_total_grams)/(cos(request.ownCrazyflie.roll)*cos(request.ownCrazyflie.pitch)*1000.0);
......@@ -847,10 +855,23 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// > Compute the feed-forward force
float feed_forward_thrust_per_motor = F_in_newtons / 4.0; //m_mass_total_grams * 9.81/(1000*4);
// > Put in the per motor commands
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
// Only for motor wearing test
// if(tookOffFlag && thrust_factor > 0){
// thrust_factor -= 1e-4;
// }
thrust_factor = 1.0;
response.controlOutput.motorCmd1 = thrust_factor * computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd2 = thrust_factor * computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd3 = thrust_factor * computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd4 = thrust_factor * computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
// Specify that this controller is a rate controller
......@@ -934,10 +955,11 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
ms_coordinates[2] = trajectory_setpoint[2];
ms_coordinates[3] = trajectory_setpoint[3];
/*
ms_velocity[0] = trajectory_velocity[0];
ms_velocity[1] = trajectory_velocity[1];
ms_velocity[2] = trajectory_velocity[2];
*/
ms_velocity[0] = 0;
ms_velocity[1] = 0;
......@@ -1088,7 +1110,7 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
msg_setpoint.z = ms_coordinates[2];
msg_setpoint.yaw = ms_coordinates[3];
dronexSetpointToGUIPublisher.publish(msg_setpoint);
//dronexSetpointToGUIPublisher.publish(msg_setpoint);
......@@ -1125,12 +1147,10 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
void calculateTrajectory(){
trajectory_x_radius = 0.2;//(area.xmax - area.xmin)/2.0;
ROS_INFO_STREAM("area_x (max,min) " << area.xmax << ", " << area.xmin);
trajectory_y_radius = 0.4;//(area.ymax - area.ymin)/2.0;
trajectory_x_radius = 1.0;
trajectory_y_radius = 0.4;
total_time_since_start = ros::Time::now().toSec() - trajectory_start_time;
ROS_INFO_STREAM("radius (x,y) " << trajectory_x_radius << ", " << trajectory_y_radius);
// Nicht ganz an den Rand -> - 0.3
trajectory_setpoint[0] = originX + cos(2*PI/trajectory_period * total_time_since_start)*(trajectory_x_radius);
trajectory_setpoint[1] = originY + sin(2*PI/trajectory_period * total_time_since_start)*(trajectory_y_radius);
trajectory_setpoint[2] = 0.5;
......@@ -1529,6 +1549,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
debugMsg.value_1 = drone_X_vel[0];
debugMsg.value_2 = drone_X_vel[1];
debugMsg.value_3 = drone_X_vel[2];
debugMsg.value_4 = thrust_factor;
// ......................
// debugMsg.value_10 = your_variable_name;
......
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