whoenig wrote: ↑Mon Jul 23, 2018 10:33 pm
It is pretty difficult to see which part is changed and which part is default:
1. Do you run the default firmware? If not, what are the firmware changes? In any case, which exact firmware are you running?
2. Which changes did you do to crazyflie_ros?
Also, I am unclear on what the exact behavior is - is flying in the wrong direction, or is the orientation simply wrong?
Thank you so much for your answer:
1) I run the latest firmware, so the
2018.01, In the firmware I changed two things because the body-fixed frame defined in the embedded system did not match the one adopted during the simulation phase: the first is the power_distribution_stock.c
and the second is the
controller_pid.c
2) My changes in the crazyflie ros package are this:
- in the crazyflie controller.cpp I created new subscribers:
m_imu_sub (topic /crazyflie/imu): displays information coming from the IMU of the Crazyflie 2.0, in particular the gyroscope measurements and then the Euler angles (roll, pitch, yaw) coming from the Sensor Fusion algorithm embedded in the Crazyflie.
-m_lpspos_sub (topic /crazyflie/crazyflie_position): subscribes to the position estimations of the LPS system.
-m_goal_sub (topic /goal): subscribes to the data coming from the MATLAB node. When using the PID controller it represents the Euler angles setpoints. Code: Select all
m_goal_sub = nh.subscribe("/goal", 1000, &Controller::GoalCallback, this);
m_imu_sub = nh.subscribe("/crazyflie/imu", 1000, &Controller::imuCallback, this);
m_lpspos_sub = nh.subscribe("/crazyflie/crazyflie_position", 1000, &Controller::posLpsCallback, this);
the methods related to the subscribers are this:
Code: Select all
void GoalCallback( const geometry_msgs::Twist::ConstPtr& msg)
{
goal_x = msg->linear.x+x_0; //X Setpoint
goal_y = msg->linear.y+y_0; //Y Setpoint
goal_z = msg->linear.z; //Z Setpoint
goal_select = msg->angular.x; //Trajectory Selector
yaw_mode = msg->angular.y; //Select Yaw mode: angular or rate
goal_yaw = msg->angular.z; //Yaw Setpoint
}
void posLpsCallback( const geometry_msgs::PoseStamped::ConstPtr& msg)
{
actual_x = msg->pose.position.x;
actual_y = msg->pose.position.y;
actual_z = msg->pose.position.z;
}
void imuCallback(const sensor_msgs::Imu::ConstPtr& msg)
{
actual_roll = msg->angular_velocity.x*3.141592654/180;
actual_pitch = msg->angular_velocity.y*3.141592654/180;
actual_yaw = msg->angular_velocity.z*3.141592654/180;
}
And this is the iteration Method:
Code: Select all
void iteration(const ros::TimerEvent& e)
{
float dt = e.current_real.toSec() - e.last_real.toSec();
float pi = 3.141592654;
switch(goal_select) //Selected via MATLAB interface
{
case 0: //Mode Idle, do nothing and await new commands
{ //ROS_INFO("MODE IDLE");
geometry_msgs::Twist msg;
if(i==0 && actual_x!=0){ //Save initial coordinates as point (0,0)
x_0=actual_x; //x_0, y_0, z_0 come from from MATLAB
y_0=actual_y;
i++;
}
pidReset();
msg.linear.x =0;
msg.linear.y =0;
msg.linear.z=0;
msg.angular.z = 0;
m_pubNav.publish(msg);
}
break;
case 1: //Landing on demand
{
//ROS_INFO("Landing");
geometry_msgs::Twist msg;
if(land_req==0 && actual_x!=0){ //Take current coordinates as landing site
x_0=actual_x;
y_0=actual_y;
z_0=actual_z;
land_req++;
}
m_thrust =45000 + m_pidZ.update(actual_z , z_0+goal_z);
if(actual_z>0.25){
d_x = (cos(actual_yaw) * (actual_x-old_x) + sin(actual_yaw) * (actual_y-old_y))/0.01;
d_y = (-sin(actual_yaw) * (actual_x-old_x) + cos(actual_yaw) * (actual_y-old_y))/0.01;
old_x = actual_x;
old_y = actual_y;
//First we take the difference between the last value of X-Y and the present value
//Then we do the appropriate rotation if we have some yaw angle to align the axis
xy_error[0] = cos(actual_yaw) * (x_0 - actual_x) + sin(actual_yaw) * (y_0 - actual_y);
xy_error[1] = -sin(actual_yaw) * (x_0 - actual_x) + cos(actual_yaw) * (y_0 - actual_y);
msg.linear.x =m_pidX.update(d_x, xy_error[0]);
msg.linear.y =m_pidY.update(d_y, xy_error[1]);
msg.linear.z=m_thrust;
msg.angular.z = m_pidYaw.update(actual_yaw*180/pi,0);
}
else {
msg.linear.x =0;
msg.linear.y =0;
msg.linear.z=0;
msg.angular.z = 0;
}
if (m_state==1){
ROS_INFO("Emergency Stop");
msg.linear.x = 0;
msg.linear.y = 0;
msg.linear.z = 0;
msg.angular.z = 0;
}
m_pubNav.publish(msg);
}
break;
default: //Default for any trajectory other than take-off and landing
{
//ROS_INFO("Following a Trajectory");
geometry_msgs::Twist msg;
m_thrust =45000 + m_pidZ.update(actual_z , goal_z);
if(actual_z>0.25){ //After take-off
d_x = (cos(actual_yaw) * (actual_x-old_x) + sin(actual_yaw) * (actual_y-old_y))/0.01;
d_y = (-sin(actual_yaw) * (actual_x-old_x) + cos(actual_yaw) * (actual_y-old_y))/0.01;
old_x = actual_x;
old_y = actual_y;
//First we take the difference between the last value of X-Y and the present value
//Then we do the appropriate rotation if we have some yaw angle to align the axis
xy_error[0] = cos(actual_yaw) * (goal_x - actual_x) + sin(actual_yaw) * (goal_y - actual_y);
xy_error[1] = -sin(actual_yaw) * (goal_x - actual_x) + cos(actual_yaw) * (goal_y - actual_y);
//This PID takes like input the difference between the error in x-y
//and maps it to a desired velocity, which is compared with the actual linear velocity
msg.linear.x =m_pidX.update(d_x, xy_error[0]);
msg.linear.y =m_pidY.update(d_y, xy_error[1]);
msg.linear.z=m_thrust;
if(yaw_mode==0) //Yaw angular mode
msg.angular.z = m_pidYaw.update(actual_yaw*180/pi,goal_yaw);
else //Yaw rate mode
msg.angular.z=goal_yaw;
}
else{ //Before Take-off
msg.linear.z=45000;
msg.linear.x = 0;
msg.linear.y = 0;
msg.angular.z = 0;
}
if (m_state==1){
ROS_INFO("Emergency Stop");
msg.linear.x = 0;
msg.linear.y = 0;
msg.linear.z = 0;
msg.angular.z = 0;
}
m_pubNav.publish(msg);
}
break;
}
}
- The modification to the crazyflie_server.cpp are this:
Publishers:
-m_pubImu (topic /crazyflie/imu): IMU data from the Crazyflie in degrees(Euler angles) and degrees per second(Gyroscope readings).
-m_pubMotors (topic /crazyflie/motor): publishes the PWM signals sent to the motors (16 bit value). (not used in this case, I wanted to use them in the LQ controller).
Code: Select all
struct logMotor {
int m1;
int m2;
int m3;
int m4;
} __attribute__((packed));
void cmdVelChanged(
const geometry_msgs::Twist::ConstPtr& msg)
{
if (!m_isEmergency) {
float roll = msg->linear.y + m_roll_trim;
float pitch = msg->linear.x + m_pitch_trim;
float yawrate = msg->angular.z;
uint16_t thrust = std::min<uint16_t>(std::max<float>(msg->linear.z, 0.0), 60000);
m_cf.sendSetpoint(roll, pitch, yawrate, thrust);
m_sentSetpoint = true;
}
}
void run()
{
ros::NodeHandle n;
n.setCallbackQueue(&m_callback_queue);
m_subscribeCmdVel = n.subscribe(m_tf_prefix + "/cmd_vel", 1, &CrazyflieROS::cmdVelChanged, this);
.........
.........
m_pubMotors = n.advertise<std_msgs::Int32MultiArray>(m_tf_prefix + "/motor", 1000); //*****new********
}
std::function<void(uint32_t, logMotor*)> cbmotor = std::bind(&CrazyflieROS::onMotorData, this, std::placeholders::_1,std::placeholders::_2);
logBlockMotor.reset(new LogBlock<logMotor>(
&m_cf,{
{"motor", "m1"},
{"motor", "m2"},
{"motor", "m3"},
{"motor", "m4"},
}, cbmotor));
logBlockMotor->start(1); // 10ms
}
.......
void onImuData(uint32_t time_in_ms, logImu* data) {
if (m_enable_logging_imu) {
sensor_msgs::Imu msg;
if (m_use_ros_time) {
msg.header.stamp = ros::Time::now();
} else {
msg.header.stamp = ros::Time(time_in_ms / 1000.0);
}
msg.header.frame_id = m_tf_prefix + "/base_link";
msg.orientation_covariance[0] = -1;
msg.orientation.x = cos(data->yaw/2)*cos(data->pitch/2)*cos(data->roll/2) + sin(data->yaw/2)*sin(data->pitch/2)*sin(data->roll/2);
msg.orientation.y = sin(data->yaw/2)*cos(data->pitch/2)*cos(data->roll/2) - cos(data->yaw/2)*sin(data->pitch/2)*sin(data->roll/2);
msg.orientation.z = cos(data->yaw/2)*sin(data->pitch/2)*cos(data->roll/2) + sin(data->yaw/2)*cos(data->pitch/2)*sin(data->roll/2);
msg.orientation.w = cos(data->yaw/2)*cos(data->pitch/2)*sin(data->roll/2) - sin(data->yaw/2)*sin(data->pitch/2)*cos(data->roll/2);
// measured in deg/s; need to convert to rad/s
msg.angular_velocity.x = data->roll;//degToRad(data->roll);
msg.angular_velocity.y = data->pitch;//degToRad(data->pitch);
msg.angular_velocity.z = data->yaw;//degToRad(data->yaw);
// measured in mG; need to convert to m/s^2
msg.linear_acceleration.x = data->gyro_x* 9.81;
msg.linear_acceleration.y = data->gyro_y* 9.81;
msg.linear_acceleration.z = data->gyro_z* 9.81;
m_pubImu.publish(msg);
}
}
void onMotorData(uint32_t time_in_ms, logMotor* data) {
std_msgs::Int32MultiArray array;
array.data.clear();
array.data.push_back(data->m1);
array.data.push_back(data->m2);
array.data.push_back(data->m3);
array.data.push_back(data->m4);
m_pubMotors.publish(array);
}
Now, everything compiles perfectly, the topic are published and when I run the MATLAB script the propellers of the crazyflie start to run, the problem is that it seems that the crazyflie has not a controller at all. For example here:
I just want that the crazyflie goes in (x:0,y:0,z:0.5) but it starts to fly in any direction like if there is no controller and there is no position sensor, I don't think it's a MATLAB problem because in the subscribers blocks I can see perfectly the values of all the sensors.
This is inside the LPS block:
and this inside the trajectory generation:
Code: Select all
function [x,y,z,yaw_mode,yaw] = message(t,select)
x = 0;
y= 0;
z = 0;
yaw_mode = 0; % 0 for Position mode, 1 for Rate mode
yaw = 0; %If position mode, then this is the setpoint in deg
%If rate mode, then the setpoint is in deg/s
global t0;
switch(select)
case 1 %PROTECTED case, left unchanged for landing
x= 0;
y= 0;
z= -0.1*(t-t0); %specify rate in cm/s for descending
yaw_mode = 0;
yaw=0;
case 2 % Hover 0.5 meter altitude @ initial position
if(t<12)
x = 0; %The initial position is always added ROS side
y = 0; %Same here, x_0 and y_0 are automatically added in ROS
z = 0.5;
yaw_mode = 0;
yaw = 0;
end
if(t>12 & t<25)
x = 0;
y = 0;
z = 0.5;
yaw_mode = 0;
yaw = 0;
end
if(t>25)
x = 0;
y = 0;
z = 0.5;
yaw_mode = 0;
yaw = 0;
end
end
The behavior is that even if i use this values, the drone goes flies randomly. I was thinking that maybe there are some problem with that initial conditions x_0, y_0, z:0, or I need to do some other changes since I have an LPS system of bitcraze and not a VICON system. But I don't know exactly what change I should do.
Thank you for this help! I want to do the formation of three drones, but of course I need to let fly one first, and I got stuck here.