Why is there a negative in front of roll and pitch
andre-nguyen opened this issue · 1 comments
andre-nguyen commented
void HLInterface::controlCmdCallbackMavComm(const mav_msgs::RollPitchYawrateThrustConstPtr & msg)
{
asctec_hl_comm::mav_ctrl msg_old;
msg_old.type = asctec_hl_comm::mav_ctrl::acceleration;
msg_old.x = - msg->pitch;
msg_old.y = - msg->roll;
double thrust = msg->thrust.z; //thrust in [N]
//mapping from thrust to asctec command [0 1] (for Firefly)
//TODO make polynomial coefficients as parameters
//msg_old.z = -0.000667*thrust*thrust + 0.0319*thrust + 0.233;
double throttle = 0.0;
if(mav_type_ == "hummingbird"){
throttle = thrust/16.0 + 0.1;
}else if(mav_type_ == "firefly"){
throttle = thrust/36.0 + 0.1;
}else{
ROS_ERROR("Unkown vehicle type...");
return;
}
if(throttle > 0.95)
throttle = 0.95;
if(throttle < 0)
throttle = 0;
msg_old.z = throttle;
msg_old.yaw = msg->yaw_rate;
sendControlCmd(msg_old);
}
Why is there a negative in front of the roll and pitch, is this some kind of coordinate transformation?
fmina commented
Yes, the MAV body frame has z
axis aligned with the gravity vector. While in the controller the body frame has z
axis pointing upward. See here http://wiki.asctec.de/display/AR/SDK+Manual
Therefore we need to transform the coordinates.