|
|
|
|
@ -27,13 +27,18 @@ void kraftsensor_init(){
|
|
|
|
|
void do_kraftsensor(){
|
|
|
|
|
uint16_t m_data[4];
|
|
|
|
|
|
|
|
|
|
/* read 2 16bit values and merge to 32bit signed integer */
|
|
|
|
|
readReg(1,0,2);
|
|
|
|
|
if(wait_receive(2, m_data, 10))
|
|
|
|
|
if(wait_receive(2, m_data, 10)){
|
|
|
|
|
kraftsensor_valid = 0;
|
|
|
|
|
printf("modbus error\n\r");
|
|
|
|
|
}
|
|
|
|
|
else{
|
|
|
|
|
kraftsensor_valid = 1;
|
|
|
|
|
int32_t tmp = (uint32_t)m_data[0]<<16;
|
|
|
|
|
tmp |= m_data[1];
|
|
|
|
|
|
|
|
|
|
/* conversion magic to milliNewton */
|
|
|
|
|
kraftsensor_value = ((tmp + 539363)*9.81)/177.380;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
@ -50,7 +55,6 @@ uint8_t wait_receive(uint8_t len, uint16_t dest[], uint8_t timeout){
|
|
|
|
|
if(receiveOkay) { //if this fails, there was either no response or a crc error
|
|
|
|
|
if(rxbuffer[1]&0x80) { //client responded with an error code
|
|
|
|
|
//handle the error
|
|
|
|
|
printf("error\n\r");
|
|
|
|
|
return -1;
|
|
|
|
|
}
|
|
|
|
|
else {
|
|
|
|
|
|