modify filter functions to be compatible with current bobbycar code

This commit is contained in:
interfisch 2023-04-16 00:11:10 +02:00
parent 91bfeffcb3
commit 00b432942f
2 changed files with 16 additions and 8 deletions

View File

@ -1,7 +1,6 @@
#include "hoverboard-esc-serial-comm.h"
ESCSerialComm::ESCSerialComm(HardwareSerial &_serialRef) { //constructor
serialRef=&_serialRef;
wheelcircumference=0.5278; //8.4cm radius -> 0.084m*2*Pi
@ -193,20 +192,22 @@ void ESCSerialComm::updateMotorparams(unsigned long loopmillis) {
}
int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
int _sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
{
float a = *((float *)cmp1);
float b = *((float *)cmp2);
return a > b ? -1 : (a < b ? 1 : 0);
}
float filterMedian(int16_t* values) {
float ESCSerialComm::filterMedian(int16_t* values) {
float copied_values[CURRENT_FILTER_SIZE];
for(int i=0;i<CURRENT_FILTER_SIZE;i++) {
copied_values[i] = values[i]; //TODO: maybe some value filtering/selection here
}
float copied_values_length = sizeof(copied_values) / sizeof(copied_values[0]);
qsort(copied_values, copied_values_length, sizeof(copied_values[0]), sort_desc);
qsort(copied_values, copied_values_length, sizeof(copied_values[0]), _sort_desc);
float mean=copied_values[CURRENT_FILTER_SIZE/2];
for (uint8_t i=1; i<=CURRENT_MEANVALUECOUNT;i++) {
@ -303,3 +304,7 @@ float ESCSerialComm::getMaxcurR() {
unsigned long ESCSerialComm::getFeedbackInterval() {
return feedback_interval_timed;
}
bool ESCSerialComm::getControllerConnected() {
return controller_connected;
}

View File

@ -3,9 +3,7 @@
#include <Arduino.h>
int sort_desc(const void *cmp1, const void *cmp2);
float filterMedian(int16_t* values);
int _sort_desc(const void *cmp1, const void *cmp2);
#define SERIAL_CONTROL_BAUD 115200 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard)
#define START_FRAME 0xABCD // [-] Start frme definition for reliable serial communication
@ -107,6 +105,8 @@ class ESCSerialComm
unsigned long getFeedbackInterval(); //get time from last received feedback
unsigned long getTripTime(unsigned long loopmillis);
bool getControllerConnected();
void resetStatistics();
private:
@ -156,6 +156,9 @@ class ESCSerialComm
void SendSerial(int16_t uSpeedLeft, int16_t uSpeedRight);
bool ReceiveSerial();
float filterMedian(int16_t* values);
};
#endif