Here's a simplified version of the code in main that relates to the problem:
Code: Select all
static float setVal;
// Definition of Thread1 thread
static THD_FUNCTION(Thread1, arg) {
while (true) {
chMtxLock(& mt);
//setVal = 250.0f;
vertiqVelocity(setVal, setVal, setVal, setVal);
chMtxUnlock(& mt);
chThdSleepMilliseconds(10);
}
}
int main(void) {
...
while (1) {
chMtxLock(&mt);
setVal = 250.0f;
chMtxUnlock(&mt);
chThdSleepMilliseconds(1000);
}
}
Then the code in a file called VertiqInterface:
Code: Select all
...
void vertiqVelocity(float v1, float v2, float v3, float v4){
vm1.vertiqSetMotorVelocity(v1);
vm2.vertiqSetMotorVelocity(v2);
vm3.vertiqSetMotorVelocity(v3);
vm4.vertiqSetMotorVelocity(v4);
}
And then code in vertiqMotors.cc:
Code: Select all
...
void vertiqMotor::vertiqSetMotorVelocity(float m) {
if (0 == m) {
// When m is 0, then set the velocity to MINSPEED
vertiqSetVelocity(MINSPEED);
} else {
// Otherwise, set the velocity proportionately between MINSPEED and
// MAXSPEED
//vertiqSetVelocity((float) (std::clamp(m, 0.0f, 1.0f) *
// (float) (MAXSPEED - MINSPEED) +
// (float) MINSPEED));
vertiqSetVelocity((float) m);
}
}
int8_t vertiqMotor::vertiqSetVelocity(float val) {
//uint32_t * val1 = (uint32_t *)((void *) (& setVal));
uint32_t val1;
memcpy(&val1, &val, 4);
chprintf(chpu, "Float into vSV: %x\r\n", val1);
prop.ctrl_velocity_.set(com, val);
//Interface with hardware:
return vertiqWrite();
}
int8_t vertiqMotor::vertiqWrite(void) {
uint8_t packet_buf[64]; // Buffer for packet
uint8_t length = 0; // Length of packet
// Get the packet from the com interface and place it into the packet buffer
if (com.GetTxBytes(packet_buf, length)) {
uint32_t * val1 = (uint32_t *)((void *) (packet_buf + length - 6));
//chprintf(chpu, "Float in write: %x\r\n", * val1);
sdWrite(chp, (const uint8_t *) packet_buf, length);
return 0;
} else {
return -1;
}
}
Both the output from chprintf and sdWrite will look like a partial double in this circumstance as written here. However if setVal gets written to the spot where it is commented out at the top of this code, then the output looks like a normal single precision float.