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

Added pitch and roll baselines.

parent ae0e2287
......@@ -739,15 +739,26 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
}
// Calculate the angle in x-y plane of DroneX for transformation to local coordinates
float droneXAngle = ;
ms_velocity_norm = sqrt(ms_velocity[0]^2 * ms_velocity[1]^2);
if(ms_velocity_norm != 0){
if (ms_velocity[1] >= 0)
{
droneXAngle = arccos(ms_velocity[0] / ms_velocity_norm);
}else{
droneXAngle = -arccos(ms_velocity[0] / ms_velocity_norm);
}
}
//float droneXAngle = ;
// Calculcate Roll and Pitch Baseline, which comes from the moving mothership
float rollAngle_baseline = 1;
float pitchAngle_baseline = 1;
// TODO
rollAngle_forResponse += rollAngle_baseline;
pitchAngle_forResponse += pitchAngle_forResponse;
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]);
// Calculate the Force Feedforward
......
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