I never dared to doubt that 
readable and comprehensible! 
great work!
will the BP commands be thread-safe or would one need mutexes additionally?
Since most robots don’t need multiple threads, making it thread-safe is not a priority. You will need to add mutexes into the user program as necessary.
actually most serious robots urgently need multithreading, but as most of the motor control is supposed to be performed by the HATs and not on the SoC I think/hope it will be feasable though 
e.g., just a couple of sensors yet, 20 more are still missing…:
void* thread0Go (void* ) { // low priority: display values // low priority: display values
//printf("init graph...: ");
setupGraphics();
// DEBUG
//printf(" done. \n\n");
while(_TASKS_ACTIVE_) {
cls();
displayvalues();
delay(40);
}
End();
FinishShapes();
return NULL;
}
//*************************************************************
void* thread1Go(void *) // low priority: keyboard monitoring // low priority: keyboard monitoring
{
int c, ch; // keyboard scancode, letter char
while(_TASKS_ACTIVE_) {
c=0; ch=0;
echoOff();
if (kbhit()) {
cursxy_lxt(0,11); printf(cleol_lxt);
ch=getchar();
if(ch>=32 && ch<127) {
printf("%c", ch);
fflush(stdout);
}
else if (ch<32) {
if (kbhit()) ch=getchar();
ch=0;
}
}
// DEBUG
else if(ch<32) {
c = getkbscancode();
if(c) {
if( c==1 ) { // ESC to quit program
printf("\n\n ESC pressed - program terminated by user \n");
_TASKS_ACTIVE_=0; // semaphore to stop all tasks
printf("\n wait for tasks to join main()... \n\n");
return NULL;
}
else {
cursxy_lxt(0,11); printf(cleol_lxt);
cursxy_lxt(0,10); printf(cleol_lxt);
if(_keypress_==_BSP_) printf("BSP ");
else if(_keypress_==_RET_) printf("RET ");
else if(_keypress_==_F1_) printf("F1 ");
else if(_keypress_==_F2_) printf("F2 ");
else if(_keypress_==_F3_) printf("F3 ");
else if(_keypress_==_F4_) printf("F4 ");
else if(_keypress_==_F5_) printf("F5 ");
else if(_keypress_==_F6_) printf("F6 ");
else if(_keypress_==_F7_) printf("F7 ");
else if(_keypress_==_F8_) printf("F8 ");
else if(_keypress_==_F9_) printf("F9 ");
else if(_keypress_==_F10_) printf("F10 ");
else if(_keypress_==_F11_) printf("F11 ");
else if(_keypress_==_F12_) printf("F12 ");
else if(_keypress_==_CUp_) printf("CUp ");
else if(_keypress_==_CDn_) printf("CDn ");
else if(_keypress_==_CLe_) printf("CLe ");
else if(_keypress_==_CRi_) printf("CRi ");
else if(_keypress_==_PUp_) printf("PUp ");
else if(_keypress_==_PDn_) printf("PDn ");
else if(_keypress_==_Hom_) printf("Hom ");
else if(_keypress_==_End_) printf("End ");
else if(_keypress_==_DEL_) printf("DEL ");
else if(_keypress_==_INS_) printf("INS ");
if(_keyshift_) printf(" +SHFT ");
if(_keyctrl_) printf(" +CTRL ");
if(_keyalt_) printf(" +ALT ");
printf("scancode=%6d \n", _kbscode_ );
fflush(stdout);
}
}
}
delay(50);
}
return NULL;
}
//*************************************************************
void* thread2Go (void* ) { // medium priority: motor remote control // medium priority: motor remote control
static int pwm0, pwm1, i;
int deadzone = 10;
printf(cls_lxt);
while(_TASKS_ACTIVE_) {
if( ! _REMOTE_OK_ ) {
for (i=0; i< MAXMOTORS; ++i ) {
motorCoast(i);
}
}
else {
if( _REMOTE_OK_ ) { // _REMOTE_OK_
pwm0 = recvanalog[0];
pwm1 = recvanalog[1];
if(abs(pwm0) < deadzone) pwm0=0;
if(abs(pwm1) < deadzone) pwm1=0;
motorOn(1, pwm1 );
motorOn(0, pwm0 );
// debug
cursxy_lxt(0,6);
printf(cleol_lxt);
printf("pwm0=%7d pwm1=%7d \n", pwm0, pwm1);
printf("enc0=%7d enc1=%7d \n",motor[0].motenc, motor[1].motenc);
}
if (!_TASKS_ACTIVE_) return NULL;
delay(10);
}
}
return NULL;
}
//*************************************************************
void* thread3Go (void* ) { // highest priority: 100µs encoder timer // highest priority: 100µs encoder timer
while(_TASKS_ACTIVE_) {
updateEncoders();
usleep(100);
}
return NULL;
}
//*************************************************************
void* thread4Go (void* ) { // medium priority: UART RC // medium priority: UART RC
while(_TASKS_ACTIVE_) {
UARTcomm();
}
return NULL;
}
//*************************************************************
void* thread5Go(void *) // medium priority: IMU + odometry // medium priority: IMU + odometry
{
uint8_t high_byte, low_byte;
int16_t angle16;
int32_t encLeft=0, encRight=0,
encLeftOld, encRightOld, dEncLeft=0, dEncRight=0;
double fxold=0, fyold=0, rollRW, rollLW, rollM;
double MotencHeading=0, dMencHdgRad, MencHdgRad=0; // Heading by odometry
double IMUHdgRad=0, oldIMUHdg, dIMUHdgRad, dIMUHdg; // Heading by IMU
double fHdgRad=0, oldfHdg, dfHdgRad; // Heading by sensor fusion
while(_TASKS_ACTIVE_) {
//-----------------------------------------------------------------------
// IMU Gyro Compass
oldIMUHdg = IMUHeading; // save old IMUHeading degree
high_byte = wiringPiI2CReadReg8 (fcmps11, 2) ;
low_byte = wiringPiI2CReadReg8 (fcmps11, 3) ;
//pitch = wiringPiI2CReadReg8 (fcmps11, 4) ;
//roll = wiringPiI2CReadReg8 (fcmps11, 5) ;
angle16 = high_byte << 8; // Calculate 16 bit angle degree
angle16 += low_byte;
IMUHeading = (double)angle16/10.0; // new IMUHeading degree
dIMUHdg = IMUHeading - oldIMUHdg; // delta IMU Heading degree
IMUHdgRad = IMUHeading * deg2rad; // degree Hdg => rad
dIMUHdgRad = dIMUHdg * deg2rad; // delta degree Hdg => rad
//-----------------------------------------------------------------------
// odometry
encLeftOld =encLeft; // save old encoder values
encRightOld=encRight;
encLeft = motorLeft.motenc; // get new encoder values
encRight= motorRight.motenc;
dEncLeft=encLeft-encLeftOld; // delta encooder
dEncRight=encRight-encRightOld;
rollLW= Wheelrollperencdeg*dEncLeft; // left wheel roll way
rollRW= Wheelrollperencdeg*dEncRight; // right wheel roll way
rollM=(rollRW + rollLW)/2; // center roll way
dMencHdgRad=atan2((rollLW - rollRW), Wheelgauge); // delta encoder heading !! GEO !!
//-----------------------------------------------------------------------
// fusioned Heading: to do: IMU + odometry + GPS !!!!!
fHeading = IMUHeading; // fusioned Hdg deg: IMU only << to do !
fHdgRad = IMUHdgRad; // fusioned Hdg rad
dfHdgRad = dIMUHdgRad; // fusioned delta Hdg rad
//-----------------------------------------------------------------------
analogIn[8] = round(fHeading); // fusioned Hdg deg ->-> analog array[8]
// ->-> send array back to Arduino dash board
//-----------------------------------------------------------------------
// calculate new position coordinates
fxold=fxpos; fyold=fypos; // save old pos (x,y)
fxpos = fxold + rollM*(sin( (fHdgRad +(dfHdgRad /2)))); // new xpos !! GEO !!
fypos = fyold + rollM*(cos(-(fHdgRad +(dfHdgRad /2)))); // new ypos !! GEO !!
//-----------------------------------------------------------------------
analogIn[6] = round(fxpos); // new x-pos ->-> analog array[6]
analogIn[7] = round(fypos); // new y-pos ->-> analog array[7]
// ->-> send array back to Arduino dash board
//-----------------------------------------------------------------------
MencHdgRad = MencHdgRad + dMencHdgRad; // new motor enc Heading
MotencHeading = MencHdgRad * 180.0/M_PI;
//----------------------------//
while (MotencHeading >=360) MotencHeading-=360; // odometry heading //
while (MotencHeading < 0) MotencHeading+=360; //----------------------------//
//-----------------------------------------------------------------------
delay(1); // 1 ms delay before next iteration loop
}
return NULL;
}
//*************************************************************
void* thread6Go (void* ) { // medium priority: RC heartbeat // medium priority: RC heartbeat
while(_TASKS_ACTIVE_) {
_lastbeat_ = _heartbeat_; // store old value
delay(10);
_heartbeat_ = rcrtlrecvbuf[TERM]; // read increasing number from Arduino
if(abs(_heartbeat_ - _lastbeat_) == 0) // no change:
{ if(_missedbeats_ <998) _missedbeats_++; } // transmission error count++
else if(abs(_heartbeat_ - _lastbeat_) <= 3) // small increase:
{ _missedbeats_ = 0; } // transmission ok
else if(abs(_heartbeat_ - _lastbeat_) > 3 ) // large increase:
{ if(_missedbeats_ <998) _missedbeats_++; } // transmission error count++
_REMOTE_OK_ = ( _missedbeats_ < 100 ); // missed beats tolerance
delay(20);
}
return NULL;
}
I could maybe add multi-threading support to the drivers, but I don’t think most applications would take advantage of it (it would just be extra complications). It seems that it would be much better to implement in user code according to project requirements.
for 1 single motor there would be needed just 1 thread each to control it,
i.e., not 2 different threads are supposed to control the same motor,
but all motors on all HATs (e.g., 16 different motors) must be able to be controlled by independent proprietary threads running simultaneously -
do you understand what I mean?
I understand what you mean, but most applications shouldn’t need multiple threads. For most applications there are alternative ways to write programs, so that they don’t need multiple threads.
if I needed not multiple threads then I wouldn’t use pre-emptive pthread threads (both joinable and detached).
multithreading by different pthread_ priorities is indispensible to achieve real-time behaviour by C programs, that’s finally why I need to use C + POSIX.
In my final program at least 20 threads will be running simultaneously, permanently and/or optionally, temporarily.
Do you need advice in making the program thread-safe, or are you already familiar with implementing mutexes to protect resources?
I can hardly imagine a small robotics program that need 20 threads. It sounds like either you aren’t using good programming practices, or you have a very complex robot in the works. Either way, mutexes around the driver methods should make it thread-safe. There’s nothing wrong with uses multiple threads, it’s just easy to get into trouble quickly if things aren’t protected properly.
Point is, you’re welcome to use multiple threads, just make sure you protect the resources with mutexes.
1st, it’s a subsumption program architecture setting up different behaviours by different priorities - are you familiar with subsumption?
All behaviours are running simultaneously, controlled by 1 arbitrator thread controlling the priorities.
So 8 different behaviours (e.g., RC, cruise, avoid, search, follow, grab, transport, recharge, …: to be continued…) would need 8 parallel threads + 1 arbitrator.
The behaviours control the actions of the propulsion and of the robot arm, depending on sensor events.
Then 1 USB RC task,
1 proprietary highspeed encoder task
2 i2c muxer tasks (i2c-0 + i2c-1),
1 UART muxer task,
1 Lego sensor reading task,
1 keyboard task,
1 heartbeat task
1 text display task
1 graphic display task
1 navigator task (sensor fusion: odometry, IMU. GPS, pozyx)
1 high priority emergency stop task
optionally several PID control tasks, 1 for each motor for the target and 1 for each motor speed.
That’s what I already have
so when woud one have to use mutexes when accessing different motors and different sensors from different BrickPi Hats by different threads?
to rephrase my question:
for using multithreading, will I have to use
int pthread_mutex_lock (pthread_mutex_t *mutex);
// variable access, sensor reading, motor control, SPI cmd
int pthread_mutex_unlock (pthread_mutex_t *mutex);
for each BrickPi3 variable access, sensor reading, motor control, or SPI cmd from either single thread?
Yes, you will need to use mutexes to prevent multiple threads from trying to communicate with the BrickPi3 at exactly the same time.
@matt:
hi,
unfortunately it’s not true what you wrote:
I think you would want to use more like 17mm or 18mm tall (rather than 20mm), but those should work.
17mm brass spaces are not possible as the stacking header is just 14mm high,
and even 14mm don’t work as it now turned out because there are only 2 holes for them (the other 2 required ones are covered by the RJ11 plugs!)
Pictures see here:
http://www.mindstormsforum.de/viewtopic.php?f=78&t=9054&p=70955#p70955
The BrickPi3 header is 13.75mm (from the bottom of the PCB to the bottom of the female header block). The Raspberry Pi header is 2.5mm from the PCB to the top of the plastic. 13.75mm+2.5mm=16.25mm between PCBs if they were fully seated. Stacking two BrickPi3’s together, I measure 16mm between the PCBs. I recommended 17mm or 18mm standoffs, and I still think that’s ideal. If you are stacking it on a different board (rather than directly on a RPi) you will might need to use a different length.
There are only two holes, because there’s not room for four with all those motor and sensor ports. In the standard configuration (using the acrylic case) the PCB mounting holes aren’t even used at all. Without the case, two posts with the header should provide adequate support.
yes, I shortened the standoffs by about 2-3mm, now they fit to the Brickpi stacking header.
Instead, 17mm fit to the larger stacking header of the PCB HAT.
By just taking 2 of the 14mm ones then they actually are not very stable, it’s bending off at the the unfixed side.
I will take some rubber bands then additionally at this opposite side instead, I guess that will work.
Those nylon standoffs won’t be as rigid as brass, but it should be fine.
yes, I think so, too 
now what I finally encountered is that the Lego-compatible plates cannot be attached when several HATs and shields are mounted, because of the missing 2 holes on the small USB- and LAN-plug side of the racks.
By the current design the Lego plates can’t be mounted at all to multiple stacked Brickpi3’s any more, and so the racks can’t be easily integrated into Lego models any longer.
Then that either 40pin B+ GPIO headers can’t be mounted on top is also complicated for a future usability, especially for multiple BrickPis and multiple additional standard Raspi HATs.
The 26 pin A/B GPIO header instead is absolutely outdated.
So for a future BrickPi4 design it is very wishful if not actually even compellingly required to have all 4 standard holes for mounting by 4 standoffs each and a B+/2/3 compatible 40 pin header, plus 4-hole Lego plates for the top and the bottom.
Of course that would require a reworking of the entire BrickPi board design.
OTOH it wouldn’t matter to increase the size of the BrickPis by a few mm or cm in width and length to provide both the 40pin headers, the 4 mounting holes, and the 4+4 RJ11 plugs as well.