/**
* Reads data from the QRNG into the soft state buffer, and then transfer this
* buffer to userland using `uiomove', which is the same `uio' abstraction as
* in FreeBSD. 64-bit memory is managed by `uiomove'.
*/
static int quantis_read(dev_t dev, struct uio *uio, cred_t *credp)
{
quantis_soft_state_t* soft_state;
int ret;
int toread, len;
LOG_DEBUG2("read %d bytes from dev %d\n", uio->uio_resid, getminor(dev));
soft_state = (quantis_soft_state_t*)ddi_get_soft_state(quantis_soft_state_p,
getminor(dev));
mutex_enter(&soft_state->mutex);
toread = min(uio->uio_resid, (int)sizeof(soft_state->buffer));
len = quantis_rng_read(soft_state, soft_state->buffer, toread);
LOG_DEBUG2("got %d bytes, max %d\n", len, toread);
if (len < toread)
{
ret = ENXIO;
}
else
{
ret = uiomove(soft_state->buffer, len, UIO_READ, uio);
}
mutex_exit(&soft_state->mutex);
return ret;
}
void quantis_reg_set(quantis_pci_device* qdev,
quantis_register reg,
quantis_register_value value)
{
char msg[MAX_MSG_LEN];
LOG_DEBUG2("In quantis_reg_set with reg=%d and value=%d\n", reg, value);
if (reg % 4 != 0)
{
snprintf(msg,
MAX_MSG_LEN,
"Offset (%d) in the registers array is not divisible by 4. This could crash the driver.\n",
reg);
QUANTIS_WARN(msg);
}
ddi_put32(qdev->regs_handle,
(quantis_register_value *)(qdev->regs + reg),
value);
}
char *playlist_script_get_filename(void *data) {
script_playlist *pl = data;
char *prog = pl->program;
FILE *pipe;
char *buf = calloc(1,1024);
if(!buf)
return NULL;
pipe = popen(prog, "r");
if(!pipe) {
LOG_ERROR1("Couldn't open pipe to program \"%s\"", prog);
return NULL;
}
if(fgets(buf, 1024, pipe) == NULL) {
LOG_ERROR1("Couldn't read filename from pipe to program \"%s\"", prog);
free(buf);
pclose(pipe);
return NULL;
}
pclose(pipe);
if(buf[0] == '\n' || (buf[0] == '\r' && buf[1] == '\n')) {
LOG_ERROR1("Got newlines instead of filename from program \"%s\"", prog);
free(buf);
return NULL;
}
if(buf[strlen(buf)-1] == '\n')
buf[strlen(buf)-1] = 0;
else
LOG_WARN1("Retrieved overly long filename \"%s\" from script, this may fail", buf);
/* De-f**k windows filenames. */
if(strlen(buf) > 0 && buf[strlen(buf)-1] == '\r')
buf[strlen(buf)-1] = 0;
LOG_DEBUG2("Program/script (\"%s\") returned filename \"%s\"", prog, buf);
return buf;
}
long Cx_StringTable::RegisterFile(const std::wstring& filename)
{
Cx_Interface<Ix_ConfigXml> pIFFile(CLSID_ConfigXmlFile);
if (pIFFile.IsNull())
{
return 0;
}
pIFFile->SetFileName(filename.c_str());
LOG_DEBUG2(LOGHEAD L"IDS_LOAD_STRFILE", PathFindFileNameW(filename.c_str()));
long count = 0;
for (int i = 0; i < 99; i++)
{
CConfigIOSection sec (pIFFile->GetData()->GetSectionByIndex(NULL, L"module", i));
ITEM item;
item.file = pIFFile->GetData();
item.group = sec;
item.module = sec->GetString(L"name");
if (item.module.empty())
break;
if (Find(item.module) == m_groups.end())
{
m_groups.push_back(item);
count++;
}
else
{
LOG_WARNING2(LOGHEAD L"IDS_IGNORE_STRGROUP", item.module);
}
}
return count;
}
void thread_mutex_unlock_c(mutex_t *mutex, int line, char *file)
{
#ifdef DEBUG_MUTEXES
thread_type *th = thread_self();
if (!th) {
LOG_ERROR3("No record for %u in unlock [%s:%d]", thread_self(), file, line);
}
LOG_DEBUG5("Unlocking %p (%s) on line %d in file %s by thread %d", mutex, mutex->name, line, file, th ? th->thread_id : -1);
mutex->line = line;
# ifdef CHECK_MUTEXES
if (th) {
int locks = 0;
avl_node *node;
mutex_t *tmutex;
_mutex_lock(&_mutextree_mutex);
while (node) {
tmutex = (mutex_t *)node->key;
if (tmutex->mutex_id == mutex->mutex_id) {
if (tmutex->thread_id != th->thread_id) {
LOG_ERROR7("ILLEGAL UNLOCK (%d != %d) on mutex [%s] in file %s line %d by thread %d [%s]", tmutex->thread_id, th->thread_id,
mutex->name ? mutex->name : "undefined", file, line, th->thread_id, th->name);
_mutex_unlock(&_mutextree_mutex);
return;
}
} else if (tmutex->thread_id == th->thread_id) {
locks++;
}
node = avl_get_next (node);
}
if ((locks > 0) && (_multi_mutex.thread_id != th->thread_id)) {
/* Don't have double mutex, has more than this mutex left */
LOG_WARN("(%d != %d) Thread %d [%s] tries to unlock a mutex [%s] in file %s line %d, without owning double mutex!",
_multi_mutex.thread_id, th->thread_id, th->thread_id, th->name, mutex->name ? mutex->name : "undefined", file, line);
}
_mutex_unlock(&_mutextree_mutex);
}
# endif /* CHECK_MUTEXES */
_mutex_unlock(mutex);
_mutex_lock(&_mutextree_mutex);
LOG_DEBUG2("Unlocked %p by thread %d", mutex, th ? th->thread_id : -1);
mutex->line = -1;
if (mutex->thread_id == th->thread_id) {
mutex->thread_id = MUTEX_STATE_NOTLOCKED;
}
_mutex_unlock(&_mutextree_mutex);
#else
_mutex_unlock(mutex);
#endif /* DEBUG_MUTEXES */
}
void thread_mutex_lock_c(mutex_t *mutex, int line, char *file)
{
#ifdef DEBUG_MUTEXES
thread_type *th = thread_self();
if (!th) LOG_WARN("No mt record for %u in lock [%s:%d]", thread_self(), file, line);
LOG_DEBUG5("Locking %p (%s) on line %d in file %s by thread %d", mutex, mutex->name, line, file, th ? th->thread_id : -1);
# ifdef CHECK_MUTEXES
/* Just a little sanity checking to make sure that we're locking
** mutexes correctly
*/
if (th) {
int locks = 0;
avl_node *node;
mutex_t *tmutex;
_mutex_lock(&_mutextree_mutex);
node = avl_get_first (_mutextree);
while (node) {
tmutex = (mutex_t *)node->key;
if (tmutex->mutex_id == mutex->mutex_id) {
if (tmutex->thread_id == th->thread_id) {
/* Deadlock, same thread can't lock the same mutex twice */
LOG_ERROR7("DEADLOCK AVOIDED (%d == %d) on mutex [%s] in file %s line %d by thread %d [%s]",
tmutex->thread_id, th->thread_id, mutex->name ? mutex->name : "undefined", file, line, th->thread_id, th->name);
_mutex_unlock(&_mutextree_mutex);
return;
}
} else if (tmutex->thread_id == th->thread_id) {
/* Mutex locked by this thread (not this mutex) */
locks++;
}
node = avl_get_next(node);
}
if (locks > 0) {
/* Has already got a mutex locked */
if (_multi_mutex.thread_id != th->thread_id) {
/* Tries to lock two mutexes, but has not got the double mutex, norty boy! */
LOG_WARN("(%d != %d) Thread %d [%s] tries to lock a second mutex [%s] in file %s line %d, without locking double mutex!",
_multi_mutex.thread_id, th->thread_id, th->thread_id, th->name, mutex->name ? mutex->name : "undefined", file, line);
}
}
_mutex_unlock(&_mutextree_mutex);
}
# endif /* CHECK_MUTEXES */
_mutex_lock(mutex);
_mutex_lock(&_mutextree_mutex);
LOG_DEBUG2("Locked %p by thread %d", mutex, th ? th->thread_id : -1);
mutex->line = line;
if (th) {
mutex->thread_id = th->thread_id;
}
_mutex_unlock(&_mutextree_mutex);
#else
_mutex_lock(mutex);
#endif /* DEBUG_MUTEXES */
}
/**
* Check audio parameters against read EDID structure to ensure
* compatibility with sink.
* @param audio audio parameters data structure
*/
static int api_CheckParamsAudio(audioParams_t * audio)
{
unsigned i = 0;
int bitSupport = -1;
shortAudioDesc_t sad;
speakerAllocationDataBlock_t allocation;
u8 errorCode = TRUE;
int valid = FALSE;
/* array to translate from AudioInfoFrame code to EDID Speaker Allocation data block bits */
const u8 sadb[] =
{ 1, 3, 5, 7, 17, 19, 21, 23, 9, 11, 13, 15, 25, 27, 29, 31, 73, 75, 77,
79, 33, 35, 37, 39, 49, 51, 53, 55, 41, 43, 45, 47 };
LOG_TRACE();
if (!api_EdidSupportsBasicAudio())
{
error_Set(ERR_SINK_DOES_NOT_SUPPORT_AUDIO);
printk("Sink does NOT support audio");
return FALSE;
}
/* check if audio type supported */
for (i = 0; i < api_EdidSadCount(); i++)
{
api_EdidSad(i, &sad);
if (audioParams_GetCodingType(audio) == shortAudioDesc_GetFormatCode(
&sad))
{
bitSupport = i;
break;
}
}
if (bitSupport >= 0)
{
api_EdidSad(bitSupport, &sad);
/* 192 kHz| 176.4 kHz| 96 kHz| 88.2 kHz| 48 kHz| 44.1 kHz| 32 kHz */
switch (audioParams_GetSamplingFrequency(audio))
{
case 32000:
valid = shortAudioDesc_Support32k(&sad);
break;
case 44100:
valid = shortAudioDesc_Support44k1(&sad);
break;
case 48000:
valid = shortAudioDesc_Support48k(&sad);
break;
case 88200:
valid = shortAudioDesc_Support88k2(&sad);
break;
case 96000:
valid = shortAudioDesc_Support96k(&sad);
break;
case 176400:
valid = shortAudioDesc_Support176k4(&sad);
break;
case 192000:
valid = shortAudioDesc_Support192k(&sad);
break;
default:
valid = FALSE;
break;
}
if (!valid)
{
error_Set(ERR_SINK_DOES_NOT_SUPPORT_AUDIO_SAMPLING_FREQ);
LOG_WARNING2("Sink does NOT support audio sampling frequency", audioParams_GetSamplingFrequency(audio));
errorCode = FALSE;
}
if (audioParams_GetCodingType(audio) == PCM)
{
/* 24 bit| 20 bit| 16 bit */
switch (audioParams_GetSampleSize(audio))
{
case 16:
valid = shortAudioDesc_Support16bit(&sad);
break;
case 20:
valid = shortAudioDesc_Support20bit(&sad);
break;
case 24:
valid = shortAudioDesc_Support24bit(&sad);
break;
default:
valid = FALSE;
break;
}
if (!valid)
{
error_Set(ERR_SINK_DOES_NOT_SUPPORT_AUDIO_SAMPLE_SIZE);
LOG_WARNING2("Sink does NOT support audio sample size", audioParams_GetSampleSize(audio));
errorCode = FALSE;
}
}
/* check Speaker Allocation */
if (api_EdidSpeakerAllocationDataBlock(&allocation) == TRUE)
{
LOG_DEBUG2("Audio channel allocation sent", sadb[audioParams_GetChannelAllocation(audio)]);
LOG_DEBUG2("Audio channel allocation accepted", speakerAllocationDataBlock_GetSpeakerAllocationByte(&allocation));
valid = (sadb[audioParams_GetChannelAllocation(audio)]
& speakerAllocationDataBlock_GetSpeakerAllocationByte(
&allocation))
== sadb[audioParams_GetChannelAllocation(audio)];
if (!valid)
{
error_Set(ERR_SINK_DOES_NOT_SUPPORT_ATTRIBUTED_CHANNELS);
printk("Sink does NOT have attributed speakers");
errorCode = FALSE;
}
}
}
else
{
error_Set(ERR_SINK_DOES_NOT_SUPPORT_AUDIO_TYPE);
printk("Sink does NOT support audio type");
errorCode = FALSE;
}
return errorCode;
}
u8 csi_shut_down_phy(u8 shutdown)
{
LOG_DEBUG2("shutdown", shutdown);
/* active low - bit 0 */
return csi_core_write_part(PHY_SHUTDOWNZ, shutdown? 0: 1, 0, 1);
}
void max_jit_openni_XMLConfig_read(t_max_jit_openni *x, t_symbol *s, short argc, t_atom *argv)
{
long i;
t_atom OutAtoms[2];
short filePathID;
long fileType = 'TEXT', outType;
char filename[MAX_FILENAME_CHARS];
char fullyQualifiedPathname[MAX_PATH_CHARS];
XnStatus nRetVal = XN_STATUS_OK;
#ifdef _DEBUG
t_object *mypatcher;
t_symbol *mypatcherpath;
if (object_obex_lookup(x, gensym("#P"), &mypatcher) != MAX_ERR_NONE)
LOG_ERROR("error getting patcher for jit.openni");
mypatcherpath = object_attr_getsym(mypatcher, gensym("filepath"));
if ((mypatcherpath) && (mypatcherpath != gensym(""))) // if I use _sym_nothing rather than gensym("") then I get linker error LNK2001: unresolved external symbol __common_symbols
{
LOG_COMMENT2("The patcher path is %s", mypatcherpath->s_name);
}
else
{
LOG_COMMENT("error getting filepath symbol for max.jit.openni");
return;
}
#endif
if (argc == 0) // if no argument supplied, ask for file
{
if (open_dialog(filename, &filePathID, &outType, &fileType, 1))
{
// non-zero: user cancelled or error
LOG_DEBUG("error getting XML config file from dialog box for max.jit.openni");
atom_setsym(OutAtoms, gensym("<none>"));
atom_setlong(OutAtoms + 1, 0);
max_jit_obex_dumpout(x, gensym("read"), 2, OutAtoms);
return;
}
}
else if ((argc != 1) || (atom_gettype(argv) != A_SYM))
{
LOG_DEBUG("read must have only one symbol argument");
atom_setsym(OutAtoms, gensym("<none>"));
atom_setlong(OutAtoms + 1, 0);
max_jit_obex_dumpout(x, gensym("read"), 2, OutAtoms);
return;
}
else // we have exactly one symbol argument
{
strncpy_zero(filename, atom_getsym(argv)->s_name, MAX_FILENAME_CHARS);
if (locatefile_extended(filename, &filePathID, &outType, &fileType, 1))
{
LOG_DEBUG2("Could not find file", atom_getsym(argv)->s_name);
atom_setsym(OutAtoms, atom_getsym(argv));
atom_setlong(OutAtoms + 1, 0);
max_jit_obex_dumpout(x, gensym("read"), 2, OutAtoms);
return;
}
}
//Load file
atom_setsym(OutAtoms, gensym(filename));
if (path_topathname(filePathID, filename, fullyQualifiedPathname) == 0)
{
LOG_DEBUG2("asking Jitter object to load file %s", fullyQualifiedPathname);
jit_object_method(max_jit_obex_jitob_get(x), gensym("init_from_xml"), gensym(fullyQualifiedPathname), &nRetVal);
if (nRetVal)
{
atom_setlong(OutAtoms + 1, 0);
}
else
{
atom_setlong(OutAtoms + 1, 1);
}
max_jit_obex_dumpout(x, gensym("read"), 2, OutAtoms);
}
else
{
atom_setlong(OutAtoms + 1, 0);
max_jit_obex_dumpout(x, gensym("read"), 2, OutAtoms);
}
}
void *max_jit_openni_new(t_symbol *s, long argc, t_atom *argv)
{
t_max_jit_openni *x;
void *o;
long i;
x = (t_max_jit_openni*)max_jit_obex_new(max_jit_openni_class, gensym("jit_openni"));
if (x)
{
o = jit_object_new(gensym("jit_openni"), x); // instantiate jit.openni jitter object
if (o)
{
// typically, max_jit_mop_setup_simple(x, o, argc, argv) is called here to handle standard MOP max wrapper setup tasks
// however, I need to create a max only outlet between the MOP outlets and dumpout, so will use the code from MAx SDK 21.6.
max_jit_obex_jitob_set(x,o);
max_jit_obex_dumpout_set(x,outlet_new(x,NULL));
x->osc_outlet = (t_object *)outlet_new(x, NULL);
max_jit_mop_setup(x);
max_jit_mop_inputs(x);
max_jit_mop_outputs(x);
x->chrSkeletonOutputFormat = 0;
max_jit_mop_matrix_args(x,argc,argv);
max_jit_attr_args(x, argc, argv); // process attribute arguments, like auto handling of @attribute's
#ifdef _DEBUG
for (i = 0; i < argc; i++)
{
switch (atom_gettype(&(argv[i])))
{
case A_LONG:
LOG_COMMENT3("arg %ld: long (%ld)", i, atom_getlong(&(argv[i])));
break;
case A_FLOAT:
LOG_COMMENT3("arg %ld: float (%f)", i, atom_getfloat(&(argv[i])));
break;
case A_SYM:
LOG_COMMENT3("arg %ld: symbol (%s)", i, atom_getsym(&(argv[i]))->s_name);
break;
default:
LOG_WARNING2("arg %ld: forbidden argument", i);
}
}
#endif
if(RegisterJitOpenNIEventCallbacks((t_jit_openni *)max_jit_obex_jitob_get(x), max_jit_openni_post_events, &(x->pRegistrationForEvents)))
{
LOG_ERROR("jit.openni: could not register for jit.openni event callbacks");
max_jit_openni_free(x);
freeobject((t_object*)x);
x = NULL;
}
else
{
LOG_DEBUG2("jit.openni: successfully registered for jit.openni event callbacks w/ regID=%x", x->pRegistrationForEvents);
}
}
else
{
LOG_ERROR("jit.openni: could not allocate object");
max_jit_obex_free(x);
freeobject((t_object*)x);
x = NULL;
}
}
return(x);
}
static int quantis_ioctl(dev_t dev,
int cmd,
intptr_t arg,
int flags,
cred_t* credp,
int* rvalp)
{
quantis_soft_state_t* soft_state;
int ret;
LOG_DEBUG2("ioctl on dev %d, cmd %x\n", getminor(dev), cmd);
soft_state = (quantis_soft_state_t*)ddi_get_soft_state(quantis_soft_state_p,
getminor(dev));
mutex_enter(&soft_state->mutex);
switch (cmd)
{
case QUANTIS_IOCTL_GET_DRIVER_VERSION:
{
uint32_t version = QUANTIS_PCI_DRIVER_VERSION;
if (quantis_copyout_uint(arg, flags, version) < 0)
{
ret = EFAULT;
}
else
{
ret = 0;
}
break;
}
case QUANTIS_IOCTL_GET_CARD_COUNT:
{
mutex_enter(&quantis_mutex);
if (quantis_copyout_uint(arg, flags, card_count) < 0)
{
ret = EFAULT;
}
else
{
ret = 0;
}
mutex_exit(&quantis_mutex);
break;
}
case QUANTIS_IOCTL_GET_MODULES_MASK:
{
uint32_t mask = quantis_rng_modules_mask(soft_state);
if (quantis_copyout_uint(arg, flags, mask) < 0)
{
ret = EFAULT;
}
else
{
ret = 0;
}
break;
}
case QUANTIS_IOCTL_GET_BOARD_VERSION:
{
uint32_t version = quantis_rng_version(soft_state);
if (quantis_copyout_uint(arg, flags, version) < 0)
{
ret = EFAULT;
}
else
{
ret = 0;
}
break;
}
case QUANTIS_IOCTL_RESET_BOARD:
{
quantis_rng_reset(soft_state);
ret = 0;
break;
}
case QUANTIS_IOCTL_ENABLE_MODULE:
{
unsigned int modules;
if (quantis_copyin_uint(arg, flags, &modules) < 0)
{
ret = EFAULT;
}
else
{
quantis_rng_enable_modules(soft_state, modules);
ret = 0;
}
break;
}
case QUANTIS_IOCTL_DISABLE_MODULE:
{
unsigned int modules;
if (quantis_copyin_uint(arg, flags, &modules) < 0)
{
ret = EFAULT;
}
else
{
quantis_rng_disable_modules(soft_state, modules);
ret = 0;
}
break;
}
case QUANTIS_IOCTL_GET_MODULES_STATUS:
{
uint32_t status = quantis_rng_modules_status(soft_state);
if (quantis_copyout_uint(arg, flags, status) < 0)
{
ret = EFAULT;
}
else
{
ret = 0;
}
break;
}
case QUANTIS_IOCTL_SET_DEBUG_LEVEL:
default:
{
ret = EINVAL;
break;
}
} /* switch */
mutex_exit(&soft_state->mutex);
return ret;
}
//calibration function for mouse sensor
void mouse_Calibration(int lineNb, float lineDist, int turnNb)
{
int i, k;
RobotCommand cmd;
int32 DeltaUPlus[MOUSE_NUMBER];
int32 DeltaUMinus[MOUSE_NUMBER];
int32 DeltaVPlus[MOUSE_NUMBER];
int32 DeltaVMinus[MOUSE_NUMBER];
int step = 0;
mouseCalibInitStep();
//reset values of calibration
for(i=0; i<MOUSE_NUMBER; i++)
{
uint8 code;
DeltaUPlus[i] = 0;
DeltaUMinus[i] = 0;
DeltaVPlus[i] = 0;
DeltaVMinus[i] = 0;
reset(i);
//set mouse in calibration mode
code = MOUSE_CALIBRATION_MODE;
uart_SendAll(mice[i].uart, &code, sizeof(code));
}
odometrySensorUsed = MOUSE_CALIBRATION;
LOG_DEBUG("Mouse Calibration");
//make the robot run N times x meters forward and backward
for(k=0; k<lineNb; k++)
{
LOG_DEBUG1("Calibration line %d", k);
//run forward
motion_LineSpeedAcc(&cmd, lineDist, CALIBRATION_SPEED, CALIBRATION_ACCELERATION, CALIBRATION_ACCELERATION);
path_LaunchTrajectory(&cmd);
/*if(path_WaitEndOfTrajectory() != TRAJ_OK)
{
LOG_ERROR("Unable to complete calibration");
return;
}*/
//wait user input
LOG_DEBUG1("Calib : %4.2f m forward, click next when done", lineDist);
mouseCalibWaitNextStep(step++);
//accumulate values of displacement
for(i=0; i<MOUSE_NUMBER; i++)
{
DeltaUPlus[i] += getU(i);
DeltaVMinus[i] += getV(i);
reset(i);
}
//run backward
motion_LineSpeedAcc(&cmd, -lineDist, CALIBRATION_SPEED, CALIBRATION_ACCELERATION, CALIBRATION_ACCELERATION);
path_LaunchTrajectory(&cmd);
/*if(path_WaitEndOfTrajectory() != TRAJ_OK)
{
LOG_ERROR("Unable to complete calibration");
return;
}*/
//wait user input
LOG_DEBUG1("Calib : %4.2f m backward, click next when done", lineDist);
mouseCalibWaitNextStep(step++);
//accumulate values of displacement
for(i=0; i<MOUSE_NUMBER; i++)
{
DeltaUMinus[i] += getU(i);
DeltaVPlus[i] += getV(i);
reset(i);
}
}
//compute ratio for each mouse
for(i=0; i<MOUSE_NUMBER; i++)
{
float theta;
LOG_DEBUG5("Mouse %d => Du+ = %ld | Du- = %ld | Dv+ = %ld | Dv- = %ld", i, DeltaUPlus[i], DeltaUMinus[i], DeltaVPlus[i], DeltaVMinus[i]);
mice[i].RatioKU = -DeltaUMinus[i]/(float)DeltaUPlus[i];
mice[i].RatioKV = -DeltaVPlus[i]/(float)DeltaVMinus[i];
theta = atan2f( DeltaVPlus[i]-DeltaVMinus[i],
DeltaUPlus[i]-DeltaUMinus[i] );
mice[i].theta = theta;
mice[i].cosTheta = cosf(mice[i].theta);
mice[i].sinTheta = sinf(mice[i].theta);
mice[i].Delta0 = (lineNb*lineDist) / (valueVTops * ((float)DeltaUPlus[i]*cosf(theta) - (float)DeltaVMinus[i]*sinf(theta)));
LOG_DEBUG4("Ratio : coef = %f | U = %f | V = %f | Theta = %f", mice[i].Delta0, mice[i].RatioKU, mice[i].RatioKV, theta);
}
//make N * 2*Pi rotation
LOG_DEBUG("Calibration rotation");
motion_RotateSpeedAcc(&cmd, 2*M_PI*turnNb, CALIBRATION_SPEED, CALIBRATION_ACCELERATION, CALIBRATION_ACCELERATION);
path_LaunchTrajectory(&cmd);
/*if(path_WaitEndOfTrajectory() != TRAJ_OK)
{
LOG_ERROR("Unable to complete calibration");
return;
}*/
LOG_DEBUG1("Calib : %d turn anticlockwise, click next when done", turnNb);
mouseCalibWaitNextStep(step++);
//compute mouse positions
for(i=0; i<MOUSE_NUMBER; i++)
{
int32 deltaU, deltaV;
int32 deltaX, deltaY;
deltaU = getU(i);
deltaV = getV(i);
reset(i);
if(deltaU < 0)
{
deltaU *= mice[i].RatioKU;
}
if(deltaV < 0)
{
deltaV *= mice[i].RatioKV;
}
deltaX = deltaU*cosf(mice[i].theta) - deltaV*sinf(mice[i].theta);
deltaY = deltaU*sinf(mice[i].theta) + deltaV*cosf(mice[i].theta);
mice[i].RX0 = deltaY / (2*M_PI*turnNb);
mice[i].RY0 = -deltaX / (2*M_PI*turnNb);
LOG_DEBUG5("Mouse %d | dU = %ld | dV = %ld | dX = %ld | dY = %ld", i, deltaU, deltaV, deltaX, deltaY);
LOG_DEBUG2("RX = %f | RY = %f", mice[i].RX0, mice[i].RY0);
sendCalibParameters(i);
}
odometrySensorUsed = MOUSE_SENSOR;
}