void orientConsumer(void* p)
{
orientation_t orientGet = invalid;
while(1)
{
xQueueReceive(q, &orientGet, portMAX_DELAY);
uart0_puts(printOrient(orientGet));
}
}
void test_qvelToQdot() {
//constants
const int nf = 5;
const int ns = NUMSTATE(nf);
const int nv = NUMQVEL(nf);
Real statedot[ns];
Real qvel[nv],qvel_cvtback[nv];
VecEuler euler = {DEGTORAD(10),DEGTORAD(20),DEGTORAD(30)};
VecOrient orient;
Mat3 R_body_to_world;
if (WMRSIM_USE_QUATERNION)
eulerToQuat(euler,orient);
else
copyEuler(euler,orient);
orientToRot(orient,R_body_to_world);
for (int i=0; i<nv; i++) {
qvel[i] = i;
}
qvelToQdot(nf,qvel,orient,R_body_to_world,statedot);
qdotToQvel(nf,statedot,orient,R_body_to_world,qvel_cvtback); //convert back to check
//PRINT
std::cout << "orient =\n";
printOrient(orient,-1,-1);
std::cout << std::endl;
std::cout << "R_body_to_world =\n";
printMat3(R_body_to_world,-1,-1);
std::cout << std::endl;
std::cout << "qvel =\n";
printMatReal(nv,1,qvel,-1,-1);
std::cout << std::endl;
std::cout << "statedot =\n";
printMatReal(ns,1,statedot,-1,-1);
std::cout << std::endl;
std::cout << "qvel (converted back) =\n";
printMatReal(nv,1,qvel_cvtback,-1,-1);
std::cout << std::endl;
}
