Revision f8cf404d
| components/ControllerAreaNetworkRx.cpp | ||
|---|---|---|
| 5 | 5 |
#include <amiro/Constants.h> |
| 6 | 6 |
#include <amiro/ControllerAreaNetworkRx.h> |
| 7 | 7 |
|
| 8 |
#include <global.hpp> |
|
| 9 |
|
|
| 8 | 10 |
using namespace chibios_rt; |
| 9 | 11 |
using namespace amiro; |
| 10 | 12 |
|
| 13 |
extern Global global; |
|
| 14 |
|
|
| 11 | 15 |
using namespace types; |
| 12 | 16 |
using namespace amiro::constants; |
| 13 | 17 |
|
| ... | ... | |
| 36 | 40 |
msg_t ControllerAreaNetworkRx::receiveSensorVal(CANRxFrame *frame) {
|
| 37 | 41 |
int deviceId = this->decodeDeviceId(frame); |
| 38 | 42 |
|
| 39 |
// chprintf((BaseSequentialStream*) &SD1, "DeviceId: %d\r\n",deviceId);
|
|
| 43 |
// chprintf((BaseSequentialStream*) &global.sercanmux1, "DeviceId: %d\r\n",deviceId);
|
|
| 40 | 44 |
|
| 41 | 45 |
switch (deviceId) {
|
| 42 | 46 |
case CAN::PROXIMITY_RING_ID(0): |
| ... | ... | |
| 72 | 76 |
break; |
| 73 | 77 |
|
| 74 | 78 |
case CAN::PROXIMITY_FLOOR_ID: |
| 75 |
// chprintf((BaseSequentialStream*) &SD1, "CAN::PROXIMITY_FLOOR_ID");
|
|
| 79 |
// chprintf((BaseSequentialStream*) &global.sercanmux1, "CAN::PROXIMITY_FLOOR_ID");
|
|
| 76 | 80 |
if (frame->DLC == 8) {
|
| 77 | 81 |
proximityFloorValue[0] = frame->data16[0]; |
| 78 | 82 |
proximityFloorValue[1] = frame->data16[1]; |
| ... | ... | |
| 192 | 196 |
CANRxFrame rxframe; |
| 193 | 197 |
msg_t message = canReceive(this->canDriver, CAN_ANY_MAILBOX, &rxframe, TIME_IMMEDIATE); |
| 194 | 198 |
if (message == RDY_OK) {
|
| 195 |
// chprintf((BaseSequentialStream*) &SD1, "Rx Message");
|
|
| 199 |
// chprintf((BaseSequentialStream*) &global.sercanmux1, "Rx Message");
|
|
| 196 | 200 |
message = this->receiveMessage(&rxframe); |
| 197 | 201 |
if (message != RDY_OK) |
| 198 | 202 |
this->receiveSensorVal(&rxframe); |
| components/DistControl.cpp | ||
|---|---|---|
| 5 | 5 |
|
| 6 | 6 |
#include <amiro/DistControl.h> |
| 7 | 7 |
|
| 8 |
#include <global.hpp> |
|
| 9 |
|
|
| 8 | 10 |
using namespace chibios_rt; |
| 9 | 11 |
using namespace amiro; |
| 10 | 12 |
using namespace types; |
| 11 | 13 |
using namespace constants; |
| 12 | 14 |
using namespace constants::DiWheelDrive; |
| 13 | 15 |
|
| 16 |
extern Global global; |
|
| 17 |
|
|
| 14 | 18 |
DistControl::DistControl(MotorControl* mc, MotorIncrements* mi) |
| 15 | 19 |
: BaseStaticThread<256>(), |
| 16 | 20 |
motorControl(mc), |
| ... | ... | |
| 142 | 146 |
|
| 143 | 147 |
/* |
| 144 | 148 |
if (time-printTime > MS2ST(100)) {
|
| 145 |
chprintf((BaseSequentialStream*) &SD1, "dist = %i um, angle = %i urad, ed = %i um, ea = %i, v = %i um/s, w = %i urad/s\n", realDistance, realAngle, errorDistance, errorAngle, targetVelocity.x, targetVelocity.w_z);
|
|
| 149 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "dist = %i um, angle = %i urad, ed = %i um, ea = %i, v = %i um/s, w = %i urad/s\n", realDistance, realAngle, errorDistance, errorAngle, targetVelocity.x, targetVelocity.w_z);
|
|
| 146 | 150 |
printTime = time; |
| 147 | 151 |
} |
| 148 | 152 |
*/ |
| components/Lidar.cpp | ||
|---|---|---|
| 1 | 1 |
#include <amiro/Lidar.h> |
| 2 | 2 |
|
| 3 |
#include <global.hpp> |
|
| 4 |
|
|
| 3 | 5 |
using namespace chibios_rt; |
| 4 | 6 |
using namespace amiro; |
| 5 | 7 |
|
| 8 |
extern Global global; |
|
| 9 |
|
|
| 6 | 10 |
uint8_t Lidar::scannedData[NUMBER_OF_CHARACTERS + 1] = {};
|
| 7 | 11 |
|
| 8 | 12 |
Lidar::Lidar(const uint8_t boardId, Lidar::SETUP setup) |
| ... | ... | |
| 49 | 53 |
flushSD2InputQueue(); |
| 50 | 54 |
|
| 51 | 55 |
// Configure LIDAR serial interface speed |
| 52 |
// chprintf((BaseSequentialStream*) &SD1, "Speed switch to " STR(SD_SPEED) "\n");
|
|
| 56 |
// chprintf((BaseSequentialStream*) &global.sercanmux1, "Speed switch to " STR(SD_SPEED) "\n");
|
|
| 53 | 57 |
chprintf((BaseSequentialStream*) &SD2, "SS" STR(SD_SPEED_PREFIX) STR(SD_SPEED) LF); |
| 54 | 58 |
|
| 55 | 59 |
// Check if the switch went well, otherwise terminate the thread |
| 56 | 60 |
if (checkDataString("SS" STR(SD_SPEED_PREFIX) STR(SD_SPEED) "\n00P\n\n")) {
|
| 57 |
chprintf((BaseSequentialStream*) &SD1, "Lidar speed switch OK\n");
|
|
| 61 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "Lidar speed switch OK\n");
|
|
| 58 | 62 |
// Configure serial interface of STM32 |
| 59 | 63 |
sdStop(&SD2); |
| 60 | 64 |
SerialConfig sdLidarconf = { SD_SPEED, 0, 0, 0 };
|
| 61 | 65 |
sdStart(&SD2, &sdLidarconf); |
| 62 | 66 |
} else {
|
| 63 |
chprintf((BaseSequentialStream*) &SD1, "Lidar speed switch NOT OK: Terminating Lidar \n");
|
|
| 67 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "Lidar speed switch NOT OK: Terminating Lidar \n");
|
|
| 64 | 68 |
palWritePad(GPIOB, GPIOB_LASER_EN, PAL_LOW); |
| 65 | 69 |
return -1; |
| 66 | 70 |
} |
| ... | ... | |
| 200 | 204 |
--this->dataIdx; |
| 201 | 205 |
this->lastInput = this->newInput; |
| 202 | 206 |
} |
| 203 |
//chprintf((BaseSequentialStream*) &SD1, "%d ", this->newInput);
|
|
| 207 |
//chprintf((BaseSequentialStream*) &global.sercanmux1, "%d ", this->newInput);
|
|
| 204 | 208 |
} else {
|
| 205 | 209 |
step = FAIL; |
| 206 | 210 |
} |
| ... | ... | |
| 216 | 220 |
break; |
| 217 | 221 |
case DATA_SHOW: |
| 218 | 222 |
// Show the decoded data |
| 219 |
chprintf((BaseSequentialStream*) &SD1, "\n%d", this->dataCounter);
|
|
| 223 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\n%d", this->dataCounter);
|
|
| 220 | 224 |
for (uint32_t idx=0; idx < this->dataIdx; idx+=2) {
|
| 221 |
chprintf((BaseSequentialStream*) &SD1, "\n%d", *((uint16_t*) &(Lidar::scannedData[idx])));
|
|
| 225 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\n%d", *((uint16_t*) &(Lidar::scannedData[idx])));
|
|
| 222 | 226 |
} |
| 223 | 227 |
step = FINISH; |
| 224 | 228 |
break; |
| ... | ... | |
| 252 | 256 |
if (lastInput == 10 && newInput == 10) {
|
| 253 | 257 |
return; |
| 254 | 258 |
} else {
|
| 255 |
chprintf((BaseSequentialStream*) &SD1, "%c", newInput);
|
|
| 259 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%c", newInput);
|
|
| 256 | 260 |
} |
| 257 | 261 |
lastInput = newInput; |
| 258 | 262 |
} else {
|
| 259 |
chprintf((BaseSequentialStream*) &SD1, "TIMEOUT\n", newInput);
|
|
| 263 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "TIMEOUT\n", newInput);
|
|
| 260 | 264 |
return; |
| 261 | 265 |
} |
| 262 | 266 |
} |
| ... | ... | |
| 291 | 295 |
|
| 292 | 296 |
void Lidar::printDetails() {
|
| 293 | 297 |
|
| 294 |
chprintf((BaseSequentialStream*) &SD1, "Print sensor details:\n");
|
|
| 298 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "Print sensor details:\n");
|
|
| 295 | 299 |
|
| 296 | 300 |
// Tell the sensor to transmit its details |
| 297 | 301 |
chprintf((BaseSequentialStream*) &SD2, "VV\n"); |
| ... | ... | |
| 302 | 306 |
|
| 303 | 307 |
void Lidar::printSpecification() {
|
| 304 | 308 |
|
| 305 |
chprintf((BaseSequentialStream*) &SD1, "Print sensor specification:\n");
|
|
| 309 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "Print sensor specification:\n");
|
|
| 306 | 310 |
|
| 307 | 311 |
// Tell the sensor to transmit its specifications |
| 308 | 312 |
chprintf((BaseSequentialStream*) &SD2, "PP\n"); |
| ... | ... | |
| 312 | 316 |
} |
| 313 | 317 |
|
| 314 | 318 |
void Lidar::printInformation() {
|
| 315 |
chprintf((BaseSequentialStream*) &SD1, "Print sensor information:\n");
|
|
| 319 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "Print sensor information:\n");
|
|
| 316 | 320 |
|
| 317 | 321 |
// Tell the sensor to transmit its information |
| 318 | 322 |
chprintf((BaseSequentialStream*) &SD2, "II\n"); |
| components/MotorControl.cpp | ||
|---|---|---|
| 232 | 232 |
System::getTime() - this->wheelCalibrationTime > 1500) {
|
| 233 | 233 |
this->motorCalibrationFactor = (float)this->rightWValues[0]/(float)this->leftWValues[0]; |
| 234 | 234 |
|
| 235 |
chprintf((BaseSequentialStream*) &SD1, "motorCalibrationFactor = %f \n" ,this->motorCalibrationFactor);
|
|
| 236 |
chprintf((BaseSequentialStream*) &SD1, "rw = %i \n" ,this->rightWValues[0]);
|
|
| 237 |
chprintf((BaseSequentialStream*) &SD1, "lw = %i \n" ,this->leftWValues[0]);
|
|
| 235 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "motorCalibrationFactor = %f \n" ,this->motorCalibrationFactor);
|
|
| 236 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "rw = %i \n" ,this->rightWValues[0]);
|
|
| 237 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "lw = %i \n" ,this->leftWValues[0]);
|
|
| 238 | 238 |
|
| 239 | 239 |
this->pwmPercentage[LEFT_WHEEL] = 0; |
| 240 | 240 |
this->pwmPercentage[RIGHT_WHEEL] = 0; |
| ... | ... | |
| 257 | 257 |
if (ziegler && ziegler2){
|
| 258 | 258 |
this->targetVelocity.x = 200000 * ((zieglerHelp%2 == 0) ? 1 : -1); |
| 259 | 259 |
this->pGain = 1000 + 100 * zieglerHelp; |
| 260 |
chprintf((BaseSequentialStream*) &SD1, "pgain = %i \n" , this->pGain);
|
|
| 260 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "pgain = %i \n" , this->pGain);
|
|
| 261 | 261 |
zieglerHelp++; |
| 262 | 262 |
ziegler = false; |
| 263 | 263 |
ziegler2 = false; |
| ... | ... | |
| 281 | 281 |
zieglerHelp2++; |
| 282 | 282 |
if (zieglerHelp2 > 20){
|
| 283 | 283 |
this->zieglerPeriod = numberOfLastVelocitiesV * this->period / nsc; |
| 284 |
chprintf((BaseSequentialStream*) &SD1, "zieglerPeriod = %f \n" ,this->zieglerPeriod);
|
|
| 284 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "zieglerPeriod = %f \n" ,this->zieglerPeriod);
|
|
| 285 | 285 |
|
| 286 | 286 |
this->targetVelocity.x = 0; |
| 287 | 287 |
this->pGain = (int) (this->pGain* 0.6); |
| ... | ... | |
| 290 | 290 |
this->memory->setpGain(this->pGain); |
| 291 | 291 |
this->memory->setiGain(this->iGain); |
| 292 | 292 |
this->memory->setdGain(this->dGain); |
| 293 |
chprintf((BaseSequentialStream*) &SD1, "pgain = %i \n" ,this->pGain);
|
|
| 294 |
chprintf((BaseSequentialStream*) &SD1, "igain = %f \n" ,this->iGain);
|
|
| 295 |
chprintf((BaseSequentialStream*) &SD1, "dgain = %f \n" ,this->dGain);
|
|
| 293 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "pgain = %i \n" ,this->pGain);
|
|
| 294 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "igain = %f \n" ,this->iGain);
|
|
| 295 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "dgain = %f \n" ,this->dGain);
|
|
| 296 | 296 |
|
| 297 | 297 |
this->motorCalibration = true; |
| 298 | 298 |
ziegler = true; |
| ... | ... | |
| 428 | 428 |
} |
| 429 | 429 |
|
| 430 | 430 |
void MotorControl::printGains(){
|
| 431 |
chprintf((BaseSequentialStream*)&SD1, "motorCalibrationFactor %f\n", this->motorCalibrationFactor );
|
|
| 432 |
chprintf((BaseSequentialStream*)&SD1, "pGain %i\n", this->pGain );
|
|
| 433 |
chprintf((BaseSequentialStream*)&SD1, "iGain %f\n", this->iGain );
|
|
| 434 |
chprintf((BaseSequentialStream*)&SD1, "dGain %f\n", this->dGain );
|
|
| 431 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "motorCalibrationFactor %f\n", this->motorCalibrationFactor );
|
|
| 432 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "pGain %i\n", this->pGain );
|
|
| 433 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "iGain %f\n", this->iGain );
|
|
| 434 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "dGain %f\n", this->dGain );
|
|
| 435 | 435 |
} |
| 436 | 436 |
|
| 437 | 437 |
void MotorControl::resetGains() |
| components/Odometry.cpp | ||
|---|---|---|
| 9 | 9 |
#include <Matrix.h> // Matrixoperations "Matrix::*" |
| 10 | 10 |
#include <amiro/Constants.h> // Constants "constants::*" |
| 11 | 11 |
#include <chprintf.h> |
| 12 |
#include <global.hpp> |
|
| 12 | 13 |
|
| 13 | 14 |
using namespace chibios_rt; |
| 14 | 15 |
using namespace amiro; |
| 15 | 16 |
using namespace constants::DiWheelDrive; |
| 16 | 17 |
|
| 18 |
extern Global global; |
|
| 19 |
|
|
| 17 | 20 |
|
| 18 | 21 |
Odometry::Odometry(MotorIncrements* mi, L3G4200D* gyroscope) |
| 19 | 22 |
: BaseStaticThread<1024>(), |
| ... | ... | |
| 58 | 61 |
robotPosition.y = this->pY * 1e6; |
| 59 | 62 |
robotPosition.f_z = (int32_t(this->pPhi * 1e6) % piScaled) + ((this->pPhi < 0) ? piScaled : 0); // Get only the postitve angel f_z in [0 .. 2 * pi] |
| 60 | 63 |
chSysUnlock(); |
| 61 |
// chprintf((BaseSequentialStream*) &SD1, "X:%d Y:%d Phi:%d", robotPosition.x,robotPosition.y, robotPosition.f_z);
|
|
| 62 |
// chprintf((BaseSequentialStream*) &SD1, "\r\n");
|
|
| 63 |
// chprintf((BaseSequentialStream*) &SD1, "X:%f Y:%f Phi:%f", this->pX,this->pY, this->pPhi);
|
|
| 64 |
// chprintf((BaseSequentialStream*) &SD1, "\r\n");
|
|
| 64 |
// chprintf((BaseSequentialStream*) &global.sercanmux1, "X:%d Y:%d Phi:%d", robotPosition.x,robotPosition.y, robotPosition.f_z);
|
|
| 65 |
// chprintf((BaseSequentialStream*) &global.sercanmux1, "\r\n");
|
|
| 66 |
// chprintf((BaseSequentialStream*) &global.sercanmux1, "X:%f Y:%f Phi:%f", this->pX,this->pY, this->pPhi);
|
|
| 67 |
// chprintf((BaseSequentialStream*) &global.sercanmux1, "\r\n");
|
|
| 65 | 68 |
return robotPosition; |
| 66 | 69 |
} |
| 67 | 70 |
|
| ... | ... | |
| 110 | 113 |
|
| 111 | 114 |
// Calculate the odometry |
| 112 | 115 |
this->updateOdometry(); |
| 113 |
|
|
| 114 | 116 |
|
| 115 |
// chprintf((BaseSequentialStream*) &SD1, "X:%f Y:%f Phi:%f", this->pX,this->pY, this->pPhi); |
|
| 116 |
// chprintf((BaseSequentialStream*) &SD1, "\r\n"); |
|
| 117 |
// chprintf((BaseSequentialStream*) &SD1, "distance_left:%f distance_right:%f", this->distance[0],this->distance[1]); |
|
| 118 |
// chprintf((BaseSequentialStream*) &SD1, "\r\n"); |
|
| 119 | 117 |
|
| 120 |
if (time >= System::getTime()) {
|
|
| 118 |
// chprintf((BaseSequentialStream*) &global.sercanmux1, "X:%f Y:%f Phi:%f", this->pX,this->pY, this->pPhi); |
|
| 119 |
// chprintf((BaseSequentialStream*) &global.sercanmux1, "\r\n"); |
|
| 120 |
// chprintf((BaseSequentialStream*) &global.sercanmux1, "distance_left:%f distance_right:%f", this->distance[0],this->distance[1]); |
|
| 121 |
// chprintf((BaseSequentialStream*) &global.sercanmux1, "\r\n"); |
|
| 122 |
|
|
| 123 |
if (time >= System::getTime()) {
|
|
| 121 | 124 |
chThdSleepUntil(time); |
| 122 | 125 |
} else {
|
| 123 |
chprintf((BaseSequentialStream*) &SD1, "WARNING Odometry: Unable to keep track\r\n");
|
|
| 124 |
}
|
|
| 126 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "WARNING Odometry: Unable to keep track\r\n");
|
|
| 127 |
}
|
|
| 125 | 128 |
} |
| 126 | 129 |
|
| 127 | 130 |
return true; |
| ... | ... | |
| 139 | 142 |
Matrix::copy<float>(this->Cp3x3,3,3,Cp3x3,3,3); |
| 140 | 143 |
// Get the differentail gyro information and reset it |
| 141 | 144 |
angular_ud = gyro->getAngular_ud(L3G4200D::AXIS_Z); |
| 142 |
gyro->angularReset();
|
|
| 145 |
gyro->angularReset();
|
|
| 143 | 146 |
chSysUnlock(); |
| 144 | 147 |
|
| 145 | 148 |
//////////////// |
| ... | ... | |
| 237 | 240 |
// Get the current increments of the QEI |
| 238 | 241 |
MotorControl::updateIncrements(this->motorIncrements, this->increment, this->incrementDifference); |
| 239 | 242 |
// |
| 240 |
// chprintf((BaseSequentialStream*) &SD1, "\ni_right = %d \t i_left = %d", this->increment[RIGHT_WHEEL], this->increment[LEFT_WHEEL]);
|
|
| 241 |
// chprintf((BaseSequentialStream*) &SD1, "\niDiff_right = %d \t iDiff_left = %d", this->incrementDifference[RIGHT_WHEEL], this->incrementDifference[LEFT_WHEEL]);
|
|
| 243 |
// chprintf((BaseSequentialStream*) &global.sercanmux1, "\ni_right = %d \t i_left = %d", this->increment[RIGHT_WHEEL], this->increment[LEFT_WHEEL]);
|
|
| 244 |
// chprintf((BaseSequentialStream*) &global.sercanmux1, "\niDiff_right = %d \t iDiff_left = %d", this->incrementDifference[RIGHT_WHEEL], this->incrementDifference[LEFT_WHEEL]);
|
|
| 242 | 245 |
|
| 243 | 246 |
// Get the driven distance for each wheel |
| 244 | 247 |
MotorControl::updateDistance(this->incrementDifference, this->distance); |
| 245 | 248 |
|
| 246 |
// chprintf((BaseSequentialStream*) &SD1, "\nx_right = %f \t x_left = %f", this->distance[RIGHT_WHEEL], this->distance[LEFT_WHEEL]);
|
|
| 249 |
// chprintf((BaseSequentialStream*) &global.sercanmux1, "\nx_right = %f \t x_left = %f", this->distance[RIGHT_WHEEL], this->distance[LEFT_WHEEL]);
|
|
| 247 | 250 |
} |
| components/accel/lis331dlh.cpp | ||
|---|---|---|
| 6 | 6 |
#include <chprintf.h> |
| 7 | 7 |
#include <cmath> // abs() |
| 8 | 8 |
#include <amiro/Constants.h> |
| 9 |
#include <global.hpp> |
|
| 9 | 10 |
|
| 10 | 11 |
namespace amiro {
|
| 11 | 12 |
|
| 13 |
extern Global global; |
|
| 14 |
|
|
| 12 | 15 |
LIS331DLH:: |
| 13 | 16 |
LIS331DLH(HWSPIDriver *driver) : |
| 14 | 17 |
driver(driver), |
| ... | ... | |
| 238 | 241 |
this->configure(&test_accel_run_cfg); |
| 239 | 242 |
BaseThread::sleep(MS2ST(sleepBetweenMeasurementMs)); |
| 240 | 243 |
// Grep some values and build the mean value |
| 241 |
chprintf((BaseSequentialStream*) &SD1, "\nACC: Get acc std values\n");
|
|
| 244 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\nACC: Get acc std values\n");
|
|
| 242 | 245 |
int32_t stdAccValues[3] = { this->getAcceleration(LIS331DLH::AXIS_X), this
|
| 243 | 246 |
->getAcceleration(LIS331DLH::AXIS_Y), this->getAcceleration( |
| 244 | 247 |
LIS331DLH::AXIS_Z) }; |
| ... | ... | |
| 248 | 251 |
for (uint8_t i = LIS331DLH::AXIS_X; i <= LIS331DLH::AXIS_Z; i++) {
|
| 249 | 252 |
accel = int32_t(this->getAcceleration(i)); |
| 250 | 253 |
stdAccValues[i] = (stdAccValues[i] * n + accel) / (n + 1); |
| 251 |
chprintf((BaseSequentialStream*) &SD1, "%c%d:%d ", accel < 0 ? '-' : '+',
|
|
| 254 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%c%d:%d ", accel < 0 ? '-' : '+',
|
|
| 252 | 255 |
accel < 0 ? -accel : accel, stdAccValues[i]); |
| 253 | 256 |
} |
| 254 |
chprintf((BaseSequentialStream*) &SD1, "\n");
|
|
| 257 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\n");
|
|
| 255 | 258 |
} |
| 256 | 259 |
|
| 257 | 260 |
// 2. Apply negative offset |
| ... | ... | |
| 262 | 265 |
this->configure(&test_accel_run_cfg); |
| 263 | 266 |
BaseThread::sleep(MS2ST(sleepBetweenMeasurementMs)); |
| 264 | 267 |
// Grep some values and build the mean value |
| 265 |
chprintf((BaseSequentialStream*) &SD1, "\nACC: Get acc neg values\n");
|
|
| 268 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\nACC: Get acc neg values\n");
|
|
| 266 | 269 |
int16_t negAccValues[3] = { this->getAcceleration(LIS331DLH::AXIS_X), this
|
| 267 | 270 |
->getAcceleration(LIS331DLH::AXIS_Y), this->getAcceleration( |
| 268 | 271 |
LIS331DLH::AXIS_Z) }; |
| ... | ... | |
| 272 | 275 |
for (uint8_t i = LIS331DLH::AXIS_X; i <= LIS331DLH::AXIS_Z; i++) {
|
| 273 | 276 |
accel = int32_t(this->getAcceleration(i)); |
| 274 | 277 |
negAccValues[i] = (negAccValues[i] * n + accel) / (n + 1); |
| 275 |
chprintf((BaseSequentialStream*) &SD1, "%c%d:%d ", accel < 0 ? '-' : '+',
|
|
| 278 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%c%d:%d ", accel < 0 ? '-' : '+',
|
|
| 276 | 279 |
accel < 0 ? -accel : accel, negAccValues[i]); |
| 277 | 280 |
} |
| 278 |
chprintf((BaseSequentialStream*) &SD1, "\n");
|
|
| 281 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\n");
|
|
| 279 | 282 |
} |
| 280 | 283 |
|
| 281 | 284 |
// 2. Apply positive offset |
| ... | ... | |
| 286 | 289 |
this->configure(&test_accel_run_cfg); |
| 287 | 290 |
BaseThread::sleep(MS2ST(sleepBetweenMeasurementMs)); |
| 288 | 291 |
// Grep some values and build the mean value |
| 289 |
chprintf((BaseSequentialStream*) &SD1, "\nACC: Get acc pos values\n");
|
|
| 292 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\nACC: Get acc pos values\n");
|
|
| 290 | 293 |
int16_t posAccValues[3] = { this->getAcceleration(LIS331DLH::AXIS_X), this
|
| 291 | 294 |
->getAcceleration(LIS331DLH::AXIS_Y), this->getAcceleration( |
| 292 | 295 |
LIS331DLH::AXIS_Z) }; |
| ... | ... | |
| 296 | 299 |
for (uint8_t i = LIS331DLH::AXIS_X; i <= LIS331DLH::AXIS_Z; i++) {
|
| 297 | 300 |
accel = int32_t(this->getAcceleration(i)); |
| 298 | 301 |
posAccValues[i] = (posAccValues[i] * n + accel) / (n + 1); |
| 299 |
chprintf((BaseSequentialStream*) &SD1, "%c%d:%d ", accel < 0 ? '-' : '+',
|
|
| 302 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%c%d:%d ", accel < 0 ? '-' : '+',
|
|
| 300 | 303 |
accel < 0 ? -accel : accel, posAccValues[i]); |
| 301 | 304 |
} |
| 302 |
chprintf((BaseSequentialStream*) &SD1, "\n");
|
|
| 305 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\n");
|
|
| 303 | 306 |
} |
| 304 | 307 |
|
| 305 | 308 |
// Get the amplitude change and compare it to the standard values from LIS331DLH manual table 3 |
| ... | ... | |
| 308 | 311 |
const int32_t ampXmin = 120, ampYmin = 120, ampZmin = 140; |
| 309 | 312 |
|
| 310 | 313 |
// TEST |
| 311 |
chprintf((BaseSequentialStream*) &SD1, "\n\nACC: Testresult\n");
|
|
| 314 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\n\nACC: Testresult\n");
|
|
| 312 | 315 |
amp = std::abs( |
| 313 | 316 |
stdAccValues[LIS331DLH::AXIS_X] - negAccValues[LIS331DLH::AXIS_X]); |
| 314 | 317 |
if (amp < ampXmin || amp > ampXmax) |
| 315 |
chprintf((BaseSequentialStream*) &SD1, "ACC: Negative x-axis faulty\n");
|
|
| 318 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "ACC: Negative x-axis faulty\n");
|
|
| 316 | 319 |
amp = std::abs( |
| 317 | 320 |
stdAccValues[LIS331DLH::AXIS_Y] - negAccValues[LIS331DLH::AXIS_Y]); |
| 318 | 321 |
if (amp < ampYmin || amp > ampYmax) |
| 319 |
chprintf((BaseSequentialStream*) &SD1, "ACC: Negative y-axis faulty\n");
|
|
| 322 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "ACC: Negative y-axis faulty\n");
|
|
| 320 | 323 |
amp = std::abs( |
| 321 | 324 |
stdAccValues[LIS331DLH::AXIS_Z] - negAccValues[LIS331DLH::AXIS_Z]); |
| 322 | 325 |
if (amp < ampZmin || amp > ampZmax) |
| 323 |
chprintf((BaseSequentialStream*) &SD1, "ACC: Negative z-axis faulty\n");
|
|
| 326 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "ACC: Negative z-axis faulty\n");
|
|
| 324 | 327 |
amp = std::abs( |
| 325 | 328 |
stdAccValues[LIS331DLH::AXIS_X] - posAccValues[LIS331DLH::AXIS_X]); |
| 326 | 329 |
if (amp < ampXmin || amp > ampXmax) |
| 327 |
chprintf((BaseSequentialStream*) &SD1, "ACC: Positive x-axis faulty\n");
|
|
| 330 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "ACC: Positive x-axis faulty\n");
|
|
| 328 | 331 |
amp = std::abs( |
| 329 | 332 |
stdAccValues[LIS331DLH::AXIS_Y] - posAccValues[LIS331DLH::AXIS_Y]); |
| 330 | 333 |
if (amp < ampYmin || amp > ampYmax) |
| 331 |
chprintf((BaseSequentialStream*) &SD1, "ACC: Positive y-axis faulty\n");
|
|
| 334 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "ACC: Positive y-axis faulty\n");
|
|
| 332 | 335 |
amp = std::abs( |
| 333 | 336 |
stdAccValues[LIS331DLH::AXIS_Z] - posAccValues[LIS331DLH::AXIS_Z]); |
| 334 | 337 |
if (amp < ampZmin || amp > ampZmax) |
| 335 |
chprintf((BaseSequentialStream*) &SD1, "ACC: Positive z-axis faulty\n");
|
|
| 338 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "ACC: Positive z-axis faulty\n");
|
|
| 336 | 339 |
|
| 337 | 340 |
// Write back the original config |
| 338 | 341 |
this->configure(config); |
| components/bluetooth/bluetooth-iwrap.cpp | ||
|---|---|---|
| 6 | 6 |
#include <amiro/bluetooth/bluetooth-iwrap.hpp> |
| 7 | 7 |
#include <amiro/bluetooth/bluetooth-connector.hpp> |
| 8 | 8 |
|
| 9 |
#include <global.hpp> |
|
| 10 |
|
|
| 9 | 11 |
using namespace chibios_rt; |
| 10 | 12 |
using namespace amiro; |
| 11 | 13 |
|
| 14 |
extern Global global; |
|
| 15 |
|
|
| 12 | 16 |
/* |
| 13 | 17 |
* Class constructor |
| 14 | 18 |
*/ |
| ... | ... | |
| 52 | 56 |
|
| 53 | 57 |
for (size_t i = 0; i < length; i++) {
|
| 54 | 58 |
// print out received chars |
| 55 |
chSequentialStreamPut((BaseSequentialStream*) &SD1, buffer[i]);
|
|
| 59 |
chSequentialStreamPut((BaseSequentialStream*) &global.sercanmux1, buffer[i]);
|
|
| 56 | 60 |
} |
| 57 | 61 |
|
| 58 | 62 |
// reset block |
| ... | ... | |
| 135 | 139 |
|
| 136 | 140 |
size_t length = strlen(cmd); |
| 137 | 141 |
// for (size_t i = 0; i < length; i++) |
| 138 |
// chSequentialStreamPut((BaseSequentialStream*) &SD1, cmd[i]); // To display the command
|
|
| 142 |
// chSequentialStreamPut((BaseSequentialStream*) &global.sercanmux1, cmd[i]); // To display the command
|
|
| 139 | 143 |
|
| 140 | 144 |
// To send the string command to WT12 bluetooth chip |
| 141 | 145 |
iwrapTransmit(HOST_IWRAP_PLAIN_LINKID,(uint8_t*) cmd, length); |
| components/bluetooth/bluetooth-serial.cpp | ||
|---|---|---|
| 3 | 3 |
|
| 4 | 4 |
#include <amiro/bluetooth/bluetooth-serial.hpp> |
| 5 | 5 |
|
| 6 |
#include <global.hpp> |
|
| 7 |
|
|
| 6 | 8 |
using namespace chibios_rt; |
| 7 | 9 |
using namespace amiro; |
| 8 | 10 |
|
| 11 |
extern Global global; |
|
| 12 |
|
|
| 9 | 13 |
/* |
| 10 | 14 |
* Class constructor |
| 11 | 15 |
*/ |
| ... | ... | |
| 47 | 51 |
length = recv_descriptor->bluetoothDescriptorGetPayloadLength(); |
| 48 | 52 |
|
| 49 | 53 |
for (size_t i = 0; i < length; i++) |
| 50 |
chSequentialStreamPut((BaseSequentialStream*) &SD1, buffer[i]);
|
|
| 54 |
chSequentialStreamPut((BaseSequentialStream*) &global.sercanmux1, buffer[i]);
|
|
| 51 | 55 |
|
| 52 | 56 |
msg = iwrap->transport.bluetoothTransportGetStorageMailbox()->post((msg_t) recv_descriptor, TIME_INFINITE); |
| 53 | 57 |
if ((msg == RDY_RESET) || stopflag) |
| components/bluetooth/checkutility.cpp | ||
|---|---|---|
| 48 | 48 |
{
|
| 49 | 49 |
if (length > 0) |
| 50 | 50 |
{
|
| 51 |
chSequentialStreamWrite((BaseSequentialStream*)&SD1, (uint8_t*)message, length);
|
|
| 52 |
// chprintf((BaseSequentialStream*) &SD1, "[%d]\n", length);
|
|
| 51 |
chSequentialStreamWrite((BaseSequentialStream*)&global.sercanmux1, (uint8_t*)message, length);
|
|
| 52 |
// chprintf((BaseSequentialStream*) &global.sercanmux1, "[%d]\n", length);
|
|
| 53 | 53 |
} |
| 54 | 54 |
return; |
| 55 | 55 |
} |
| ... | ... | |
| 72 | 72 |
{
|
| 73 | 73 |
const uint16_t length = strlen(message); |
| 74 | 74 |
if (length >= 1 << 10) {
|
| 75 |
chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: message too long!\n", __FILE__ , __LINE__);
|
|
| 75 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): ERROR: message too long!\n", __FILE__ , __LINE__);
|
|
| 76 | 76 |
return; |
| 77 | 77 |
} |
| 78 | 78 |
|
| ... | ... | |
| 116 | 116 |
|
| 117 | 117 |
if (msg.head.length > max_length) |
| 118 | 118 |
{
|
| 119 |
chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: message too long (%d of max %d bytes)!\n", __FILE__ , __LINE__, msg.head.length, max_length);
|
|
| 119 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): ERROR: message too long (%d of max %d bytes)!\n", __FILE__ , __LINE__, msg.head.length, max_length);
|
|
| 120 | 120 |
|
| 121 | 121 |
for (uint16_t i = 0; i < msg.head.length; ++i) {
|
| 122 | 122 |
chSequentialStreamGet((BaseSequentialStream*)&SD3); |
| ... | ... | |
| 132 | 132 |
|
| 133 | 133 |
if (uint8_t(msg.head.link_id ^ msg.n_link) != 0xFFu) |
| 134 | 134 |
{
|
| 135 |
chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: message link id does not match (0x%02X vs 0x%02X)!\n", __FILE__ , __LINE__, msg.head.link_id, msg.n_link);
|
|
| 135 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): ERROR: message link id does not match (0x%02X vs 0x%02X)!\n", __FILE__ , __LINE__, msg.head.link_id, msg.n_link);
|
|
| 136 | 136 |
} |
| 137 | 137 |
|
| 138 | 138 |
return msg.head.length; |
| ... | ... | |
| 160 | 160 |
// } |
| 161 | 161 |
|
| 162 | 162 |
if (length_buf[msg_count] == 0) {
|
| 163 |
chprintf((BaseSequentialStream*) &SD1, "%s(%d): WARNING: receiving aborted after %d messages (%d bytes)\n", __FILE__ , __LINE__, msg_count, msg_pos);
|
|
| 163 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): WARNING: receiving aborted after %d messages (%d bytes)\n", __FILE__ , __LINE__, msg_count, msg_pos);
|
|
| 164 | 164 |
return msg_count; |
| 165 | 165 |
} |
| 166 | 166 |
|
| ... | ... | |
| 178 | 178 |
} |
| 179 | 179 |
|
| 180 | 180 |
if (msg_count > len_buf_size) {
|
| 181 |
chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: maximum messages received (%d)!\n", __FILE__ , __LINE__, len_buf_size);
|
|
| 181 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): ERROR: maximum messages received (%d)!\n", __FILE__ , __LINE__, len_buf_size);
|
|
| 182 | 182 |
return len_buf_size; |
| 183 | 183 |
} |
| 184 | 184 |
|
| ... | ... | |
| 201 | 201 |
{
|
| 202 | 202 |
for (i = 0; i < num_bytes; ++i) |
| 203 | 203 |
{
|
| 204 |
chprintf((BaseSequentialStream*) &SD1, "%02X ", buffer[i]);
|
|
| 204 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%02X ", buffer[i]);
|
|
| 205 | 205 |
} |
| 206 |
chprintf((BaseSequentialStream*) &SD1, "\n");
|
|
| 206 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\n");
|
|
| 207 | 207 |
} |
| 208 | 208 |
|
| 209 | 209 |
return; |
| ... | ... | |
| 217 | 217 |
{
|
| 218 | 218 |
byte = chSequentialStreamGet((BaseSequentialStream*) &SD3); |
| 219 | 219 |
if (print) {
|
| 220 |
chprintf((BaseSequentialStream*) &SD1, "%c", byte);
|
|
| 220 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%c", byte);
|
|
| 221 | 221 |
} |
| 222 | 222 |
} |
| 223 | 223 |
|
| ... | ... | |
| 233 | 233 |
byte = sdGet(&SD3); |
| 234 | 234 |
//byte = chSequentialStreamGet((BaseSequentialStream*) &SD3); |
| 235 | 235 |
if (print) {
|
| 236 |
chprintf((BaseSequentialStream*) &SD1, "0x%02X <%c>\n", byte, byte);
|
|
| 236 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "0x%02X <%c>\n", byte, byte);
|
|
| 237 | 237 |
} |
| 238 | 238 |
BaseThread::sleep(MS2ST(1)); |
| 239 | 239 |
} |
| components/gyro/l3g4200d.cpp | ||
|---|---|---|
| 7 | 7 |
#include <amiro/bus/spi/HWSPIDriver.hpp> |
| 8 | 8 |
#include <amiro/gyro/l3g4200d.hpp> |
| 9 | 9 |
#include <amiro/Constants.h> |
| 10 |
#include <global.hpp> |
|
| 10 | 11 |
|
| 11 | 12 |
using namespace chibios_rt; |
| 12 | 13 |
|
| 14 |
extern Global global; |
|
| 15 |
|
|
| 13 | 16 |
namespace amiro {
|
| 14 | 17 |
|
| 15 | 18 |
L3G4200D::L3G4200D(HWSPIDriver *driver) |
| ... | ... | |
| 34 | 37 |
this->setName("l3g4200d");
|
| 35 | 38 |
|
| 36 | 39 |
while (!this->shouldTerminate()) {
|
| 37 |
time += this->period_st;
|
|
| 40 |
time += this->period_st;
|
|
| 38 | 41 |
|
| 39 | 42 |
updateSensorData(); |
| 40 | 43 |
calcAngular(); |
| ... | ... | |
| 44 | 47 |
if (time >= System::getTime()) {
|
| 45 | 48 |
chThdSleepUntil(time); |
| 46 | 49 |
} else {
|
| 47 |
chprintf((BaseSequentialStream*) &SD1, "WARNING l3g4200d: Unable to keep track\r\n");
|
|
| 48 |
}
|
|
| 50 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "WARNING l3g4200d: Unable to keep track\r\n");
|
|
| 51 |
}
|
|
| 49 | 52 |
} |
| 50 | 53 |
return RDY_OK; |
| 51 | 54 |
} |
| ... | ... | |
| 168 | 171 |
|
| 169 | 172 |
// Reset the integration |
| 170 | 173 |
this->angularReset(); |
| 171 |
|
|
| 174 |
|
|
| 172 | 175 |
return RDY_OK; |
| 173 | 176 |
|
| 174 | 177 |
} |
| components/power/adconverter.cpp | ||
|---|---|---|
| 116 | 116 |
|
| 117 | 117 |
if (global.adc1_buffer[0] > uV2Adc(this->watchdog_threshold_uV)) |
| 118 | 118 |
{
|
| 119 |
chprintf((BaseSequentialStream*)&SD1, "%f > 9V detected: now charging\n", ((float)adc2uV(global.adc1_buffer[0]))/1000000.0f);
|
|
| 119 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "%f > 9V detected: now charging\n", ((float)adc2uV(global.adc1_buffer[0]))/1000000.0f);
|
|
| 120 | 120 |
global.robot.getPowerStatus().charging_flags.content.vsys_higher_than_9V = true; |
| 121 | 121 |
global.bq24103a[constants::PowerManagement::BAT_P7]->enable(true); |
| 122 | 122 |
global.bq24103a[constants::PowerManagement::BAT_P8]->enable(true); |
| 123 | 123 |
this->conversion_group.htr = ADC_HTR_HT; |
| 124 | 124 |
this->conversion_group.ltr = ADC_LTR(uV2Adc(this->watchdog_threshold_uV)); |
| 125 | 125 |
} else {
|
| 126 |
chprintf((BaseSequentialStream*)&SD1, "%f < 9V detected: not charging\n", ((float)adc2uV(global.adc1_buffer[0]))/1000000.0f);
|
|
| 126 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "%f < 9V detected: not charging\n", ((float)adc2uV(global.adc1_buffer[0]))/1000000.0f);
|
|
| 127 | 127 |
global.robot.getPowerStatus().charging_flags.content.vsys_higher_than_9V = false; |
| 128 | 128 |
this->conversion_group.htr = ADC_HTR(uV2Adc(this->watchdog_threshold_uV)); |
| 129 | 129 |
this->conversion_group.ltr = 0; |
| components/power/bq27500.cpp | ||
|---|---|---|
| 2 | 2 |
|
| 3 | 3 |
#include <ch.hpp> |
| 4 | 4 |
#include <chprintf.h> |
| 5 |
#include <global.hpp> |
|
| 5 | 6 |
|
| 6 | 7 |
#include <cstring> |
| 7 | 8 |
|
| ... | ... | |
| 9 | 10 |
using namespace amiro; |
| 10 | 11 |
using namespace BQ27500; |
| 11 | 12 |
|
| 13 |
extern Global global; |
|
| 14 |
|
|
| 12 | 15 |
Driver::Driver(I2CDriver &i2c_driver, const GPIO_TypeDef &batgd_pingrp, const uint8_t batgd_pin, const GPIO_TypeDef &batlow_pingrp, const uint8_t batlow_pin) : |
| 13 | 16 |
BaseSensor<BQ27500::InitData,BQ27500::CalibData>(), i2c_driver(&i2c_driver), tx_params({I2C_ADDR, NULL, 0, NULL, 0}),
|
| 14 | 17 |
batgd_pingrp(&batgd_pingrp), batgd_pin(batgd_pin), batlow_pingrp(&batlow_pingrp), batlow_pin(batlow_pin) |
| ... | ... | |
| 26 | 29 |
msg_t |
| 27 | 30 |
Driver::init(InitData* initialization_data) |
| 28 | 31 |
{
|
| 29 |
chprintf((BaseSequentialStream*) &SD1, "%s(%d): TODO\n", __FILE__, __LINE__);
|
|
| 32 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): TODO\n", __FILE__, __LINE__);
|
|
| 30 | 33 |
return NOT_IMPLEMENTED; |
| 31 | 34 |
} |
| 32 | 35 |
|
| ... | ... | |
| 64 | 67 |
DataFlashBlock block; |
| 65 | 68 |
this->readDataFlashBlock(block, CONFIGURATION_Safety); |
| 66 | 69 |
|
| 67 |
chprintf((BaseSequentialStream*) &SD1, "%s(%d):\n", __FILE__, __LINE__);
|
|
| 70 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d):\n", __FILE__, __LINE__);
|
|
| 68 | 71 |
for(uint8_t i = 0; i < 32; ++i) {
|
| 69 |
chprintf((BaseSequentialStream*) &SD1, "%02X ", block.content.data[i]);
|
|
| 72 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%02X ", block.content.data[i]);
|
|
| 70 | 73 |
} |
| 71 |
chprintf((BaseSequentialStream*) &SD1, "\n");
|
|
| 72 |
chprintf((BaseSequentialStream*) &SD1, "%02X\n", block.content.checksum);
|
|
| 74 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\n");
|
|
| 75 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%02X\n", block.content.checksum);
|
|
| 73 | 76 |
|
| 74 |
chprintf((BaseSequentialStream*) &SD1, "%s(%d): TODO\n", __FILE__, __LINE__);
|
|
| 77 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): TODO\n", __FILE__, __LINE__);
|
|
| 75 | 78 |
return NOT_IMPLEMENTED; |
| 76 | 79 |
} |
| 77 | 80 |
#endif |
| ... | ... | |
| 89 | 92 |
{
|
| 90 | 93 |
return ST_FAIL_READ_HW_VERSION; |
| 91 | 94 |
} |
| 92 |
chprintf((BaseSequentialStream*) &SD1, "hardware version: %X%X-%X%X (0x%04X)\n", version.content.major_high, version.content.major_low, version.content.minor_high, version.content.minor_low, version.value);
|
|
| 95 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "hardware version: %X%X-%X%X (0x%04X)\n", version.content.major_high, version.content.major_low, version.content.minor_high, version.content.minor_low, version.value);
|
|
| 93 | 96 |
|
| 94 | 97 |
// read firmware version |
| 95 | 98 |
version.value = 0; |
| ... | ... | |
| 97 | 100 |
{
|
| 98 | 101 |
return ST_FAIL_READ_FW_VERSION; |
| 99 | 102 |
} |
| 100 |
chprintf((BaseSequentialStream*) &SD1, "firmware version: %X%X-%X%X (0x%04X)\n", version.content.major_high, version.content.major_low, version.content.minor_high, version.content.minor_low, version.value);
|
|
| 103 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "firmware version: %X%X-%X%X (0x%04X)\n", version.content.major_high, version.content.major_low, version.content.minor_high, version.content.minor_low, version.value);
|
|
| 101 | 104 |
|
| 102 | 105 |
// read device name |
| 103 | 106 |
uint8_t name_length = 0; |
| ... | ... | |
| 111 | 114 |
return ST_FAIL_READ_DEVICENAME; |
| 112 | 115 |
} |
| 113 | 116 |
name_buffer[name_length] = '\0'; |
| 114 |
chprintf((BaseSequentialStream*) &SD1, "device name: %s (%u characters)\n", name_buffer, name_length);
|
|
| 117 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "device name: %s (%u characters)\n", name_buffer, name_length);
|
|
| 115 | 118 |
|
| 116 | 119 |
// read the current flags |
| 117 | 120 |
Flags flags; |
| ... | ... | |
| 119 | 122 |
{
|
| 120 | 123 |
return ST_FAIL_READ_FLAGS; |
| 121 | 124 |
} |
| 122 |
chprintf((BaseSequentialStream*) &SD1, "flags: 0x%04X\n", flags.value);
|
|
| 123 |
chprintf((BaseSequentialStream*) &SD1, " OTC : %u\n", flags.content.otc);
|
|
| 124 |
chprintf((BaseSequentialStream*) &SD1, " OTD : %u\n", flags.content.otd);
|
|
| 125 |
chprintf((BaseSequentialStream*) &SD1, " CHG_INH : %u\n", flags.content.chg_inh);
|
|
| 126 |
chprintf((BaseSequentialStream*) &SD1, " XCHG : %u\n", flags.content.xchg);
|
|
| 127 |
chprintf((BaseSequentialStream*) &SD1, " FC : %u\n", flags.content.fc);
|
|
| 128 |
chprintf((BaseSequentialStream*) &SD1, " CHG : %u\n", flags.content.chg);
|
|
| 129 |
chprintf((BaseSequentialStream*) &SD1, " OCV_GD : %u\n", flags.content.ocv_gd);
|
|
| 130 |
chprintf((BaseSequentialStream*) &SD1, " WAIT_ID : %u\n", flags.content.wait_id);
|
|
| 131 |
chprintf((BaseSequentialStream*) &SD1, " BAT_DET : %u\n", flags.content.bat_det);
|
|
| 132 |
chprintf((BaseSequentialStream*) &SD1, " SOC1 : %u\n", flags.content.soc1);
|
|
| 133 |
chprintf((BaseSequentialStream*) &SD1, " SOCF : %u\n", flags.content.socf);
|
|
| 134 |
chprintf((BaseSequentialStream*) &SD1, " DSG : %u\n", flags.content.dsg);
|
|
| 125 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "flags: 0x%04X\n", flags.value);
|
|
| 126 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " OTC : %u\n", flags.content.otc);
|
|
| 127 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " OTD : %u\n", flags.content.otd);
|
|
| 128 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " CHG_INH : %u\n", flags.content.chg_inh);
|
|
| 129 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " XCHG : %u\n", flags.content.xchg);
|
|
| 130 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " FC : %u\n", flags.content.fc);
|
|
| 131 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " CHG : %u\n", flags.content.chg);
|
|
| 132 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " OCV_GD : %u\n", flags.content.ocv_gd);
|
|
| 133 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " WAIT_ID : %u\n", flags.content.wait_id);
|
|
| 134 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " BAT_DET : %u\n", flags.content.bat_det);
|
|
| 135 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " SOC1 : %u\n", flags.content.soc1);
|
|
| 136 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " SOCF : %u\n", flags.content.socf);
|
|
| 137 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " DSG : %u\n", flags.content.dsg);
|
|
| 135 | 138 |
|
| 136 | 139 |
// read the current controller status |
| 137 | 140 |
ControlStatus ctrl_status; |
| ... | ... | |
| 139 | 142 |
{
|
| 140 | 143 |
return ST_FAIL_READ_STATUS; |
| 141 | 144 |
} |
| 142 |
chprintf((BaseSequentialStream*) &SD1, "control status: 0x%04X\n", ctrl_status.value);
|
|
| 143 |
chprintf((BaseSequentialStream*) &SD1, " FAS : %u\n", ctrl_status.content.fas);
|
|
| 144 |
chprintf((BaseSequentialStream*) &SD1, " SS : %u\n", ctrl_status.content.ss);
|
|
| 145 |
chprintf((BaseSequentialStream*) &SD1, " CSV : %u\n", ctrl_status.content.csv);
|
|
| 146 |
chprintf((BaseSequentialStream*) &SD1, " CSA : %u\n", ctrl_status.content.cca);
|
|
| 147 |
chprintf((BaseSequentialStream*) &SD1, " BCA : %u\n", ctrl_status.content.bca);
|
|
| 148 |
chprintf((BaseSequentialStream*) &SD1, " HIBERNATE : %u\n", ctrl_status.content.hibernate);
|
|
| 149 |
chprintf((BaseSequentialStream*) &SD1, " SNOOZE : %u\n", ctrl_status.content.snooze);
|
|
| 150 |
chprintf((BaseSequentialStream*) &SD1, " SLEEP : %u\n", ctrl_status.content.sleep);
|
|
| 151 |
chprintf((BaseSequentialStream*) &SD1, " LDMD : %u\n", ctrl_status.content.ldmd);
|
|
| 152 |
chprintf((BaseSequentialStream*) &SD1, " RUP_DIS : %u\n", ctrl_status.content.rup_dis);
|
|
| 153 |
chprintf((BaseSequentialStream*) &SD1, " VOK : %u\n", ctrl_status.content.vok);
|
|
| 154 |
chprintf((BaseSequentialStream*) &SD1, " QEN : %u\n", ctrl_status.content.qen);
|
|
| 145 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "control status: 0x%04X\n", ctrl_status.value);
|
|
| 146 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " FAS : %u\n", ctrl_status.content.fas);
|
|
| 147 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " SS : %u\n", ctrl_status.content.ss);
|
|
| 148 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " CSV : %u\n", ctrl_status.content.csv);
|
|
| 149 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " CSA : %u\n", ctrl_status.content.cca);
|
|
| 150 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " BCA : %u\n", ctrl_status.content.bca);
|
|
| 151 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " HIBERNATE : %u\n", ctrl_status.content.hibernate);
|
|
| 152 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " SNOOZE : %u\n", ctrl_status.content.snooze);
|
|
| 153 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " SLEEP : %u\n", ctrl_status.content.sleep);
|
|
| 154 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " LDMD : %u\n", ctrl_status.content.ldmd);
|
|
| 155 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " RUP_DIS : %u\n", ctrl_status.content.rup_dis);
|
|
| 156 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " VOK : %u\n", ctrl_status.content.vok);
|
|
| 157 |
chprintf((BaseSequentialStream*) &global.sercanmux1, " QEN : %u\n", ctrl_status.content.qen);
|
|
| 155 | 158 |
|
| 156 | 159 |
// if no battery was detected, abort |
| 157 | 160 |
if (!flags.content.bat_det) |
| ... | ... | |
| 160 | 163 |
} |
| 161 | 164 |
|
| 162 | 165 |
// read the BATGD_N pin |
| 163 |
chprintf((BaseSequentialStream*) &SD1, "battery good: %s\n", (this->isBatteryGood()? "yes" : "no"));
|
|
| 166 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "battery good: %s\n", (this->isBatteryGood()? "yes" : "no"));
|
|
| 164 | 167 |
|
| 165 | 168 |
// read temperature |
| 166 | 169 |
if (this->stdCommand(STD_CMD_TEMP, val)) |
| 167 | 170 |
{
|
| 168 | 171 |
return ST_FAIL_READ_TEMP; |
| 169 | 172 |
} |
| 170 |
chprintf((BaseSequentialStream*) &SD1, "temperature: %fK (%fC)\n", float(val)/10.0f, float(val)/10.0f - 273.15f);
|
|
| 173 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "temperature: %fK (%fC)\n", float(val)/10.0f, float(val)/10.0f - 273.15f);
|
|
| 171 | 174 |
|
| 172 | 175 |
// read the full available capacity |
| 173 | 176 |
if (this->stdCommand(STD_CMD_FAC, val)) |
| 174 | 177 |
{
|
| 175 | 178 |
return ST_FAIL_READ_FAC; |
| 176 | 179 |
} |
| 177 |
chprintf((BaseSequentialStream*) &SD1, "full available capacity: %umAh\n", val);
|
|
| 180 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "full available capacity: %umAh\n", val);
|
|
| 178 | 181 |
|
| 179 | 182 |
// read the full charge capacity |
| 180 | 183 |
if (this->stdCommand(STD_CMD_FCC, val)) |
| 181 | 184 |
{
|
| 182 | 185 |
return ST_FAIL_READ_FCC; |
| 183 | 186 |
} |
| 184 |
chprintf((BaseSequentialStream*) &SD1, "full charge capacity: %umAh\n", val);
|
|
| 187 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "full charge capacity: %umAh\n", val);
|
|
| 185 | 188 |
|
| 186 | 189 |
// read the remaining capacity |
| 187 | 190 |
if (this->stdCommand(STD_CMD_RM, val)) |
| 188 | 191 |
{
|
| 189 | 192 |
return ST_FAIL_READ_RM; |
| 190 | 193 |
} |
| 191 |
chprintf((BaseSequentialStream*) &SD1, "remaining capacity capacity: %umAh\n", val);
|
|
| 194 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "remaining capacity capacity: %umAh\n", val);
|
|
| 192 | 195 |
|
| 193 | 196 |
// read the state of charge |
| 194 | 197 |
if (this->stdCommand(STD_CMD_SOC, val)) |
| 195 | 198 |
{
|
| 196 | 199 |
return ST_FAIL_READ_SOC; |
| 197 | 200 |
} |
| 198 |
chprintf((BaseSequentialStream*) &SD1, "state of charge: %3u%%\n", val);
|
|
| 201 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "state of charge: %3u%%\n", val);
|
|
| 199 | 202 |
|
| 200 | 203 |
// read voltage |
| 201 | 204 |
if (this->stdCommand(STD_CMD_VOLT, val)) |
| 202 | 205 |
{
|
| 203 | 206 |
return ST_FAIL_READ_VOLT; |
| 204 | 207 |
} |
| 205 |
chprintf((BaseSequentialStream*) &SD1, "voltage: %umV\n", val);
|
|
| 208 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "voltage: %umV\n", val);
|
|
| 206 | 209 |
|
| 207 | 210 |
// read average current |
| 208 | 211 |
if (this->stdCommand(STD_CMD_AI, val)) |
| 209 | 212 |
{
|
| 210 | 213 |
return ST_FAIL_READ_AI; |
| 211 | 214 |
} |
| 212 |
chprintf((BaseSequentialStream*) &SD1, "average current: %dmA\n", *reinterpret_cast<int8_t*>(&val));
|
|
| 215 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "average current: %dmA\n", *reinterpret_cast<int8_t*>(&val));
|
|
| 213 | 216 |
|
| 214 | 217 |
// read average power |
| 215 | 218 |
if (this->stdCommand(STD_CMD_AP, val)) |
| 216 | 219 |
{
|
| 217 | 220 |
return ST_FAIL_READ_AP; |
| 218 | 221 |
} |
| 219 |
chprintf((BaseSequentialStream*) &SD1, "average power: %dmW\n", *reinterpret_cast<int8_t*>(&val));
|
|
| 222 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "average power: %dmW\n", *reinterpret_cast<int8_t*>(&val));
|
|
| 220 | 223 |
|
| 221 | 224 |
// read the BATLOW pin |
| 222 |
chprintf((BaseSequentialStream*) &SD1, "battery low: %s\n", (this->isBatteryLow()? "yes" : "no"));
|
|
| 225 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "battery low: %s\n", (this->isBatteryLow()? "yes" : "no"));
|
|
| 223 | 226 |
|
| 224 | 227 |
// read the time to empty |
| 225 | 228 |
if (this->stdCommand(STD_CMD_TTE, val)) |
| 226 | 229 |
{
|
| 227 | 230 |
return ST_FAIL_READ_TTE; |
| 228 | 231 |
} |
| 229 |
chprintf((BaseSequentialStream*) &SD1, "time to empty: ");
|
|
| 232 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "time to empty: ");
|
|
| 230 | 233 |
if (uint16_t(~val)) {
|
| 231 |
chprintf((BaseSequentialStream*) &SD1, "%u minutes", val);
|
|
| 234 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%u minutes", val);
|
|
| 232 | 235 |
} else {
|
| 233 |
chprintf((BaseSequentialStream*) &SD1, "(not discharging)");
|
|
| 236 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "(not discharging)");
|
|
| 234 | 237 |
} |
| 235 |
chprintf((BaseSequentialStream*) &SD1, "\n");
|
|
| 238 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\n");
|
|
| 236 | 239 |
|
| 237 | 240 |
// read the time to full |
| 238 | 241 |
if (this->stdCommand(STD_CMD_TTF, val)) |
| 239 | 242 |
{
|
| 240 | 243 |
return ST_FAIL_READ_TTF; |
| 241 | 244 |
} |
| 242 |
chprintf((BaseSequentialStream*) &SD1, "time to full: ");
|
|
| 245 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "time to full: ");
|
|
| 243 | 246 |
if (uint16_t(~val)) {
|
| 244 |
chprintf((BaseSequentialStream*) &SD1, "%u minutes", val);
|
|
| 247 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%u minutes", val);
|
|
| 245 | 248 |
} else {
|
| 246 |
chprintf((BaseSequentialStream*) &SD1, "(not charging)");
|
|
| 249 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "(not charging)");
|
|
| 247 | 250 |
} |
| 248 |
chprintf((BaseSequentialStream*) &SD1, "\n");
|
|
| 251 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\n");
|
|
| 249 | 252 |
|
| 250 | 253 |
return ST_OK; |
| 251 | 254 |
} |
| ... | ... | |
| 300 | 303 |
dst = uint16_t((buffer[1] << 8) | buffer[0]); |
| 301 | 304 |
#ifndef NDEBUG |
| 302 | 305 |
} else {
|
| 303 |
chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: i2c transmit failed (%d)\n", __FILE__ , __LINE__ , res);
|
|
| 306 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): ERROR: i2c transmit failed (%d)\n", __FILE__ , __LINE__ , res);
|
|
| 304 | 307 |
#endif |
| 305 | 308 |
} |
| 306 | 309 |
|
| ... | ... | |
| 323 | 326 |
|
| 324 | 327 |
buffer[buffer[0] + 1] = '\0'; |
| 325 | 328 |
|
| 326 |
chprintf((BaseSequentialStream*) &SD1, "name: %u - %s\n", buffer[0], (char*)&buffer[1]);
|
|
| 329 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "name: %u - %s\n", buffer[0], (char*)&buffer[1]);
|
|
| 327 | 330 |
|
| 328 | 331 |
return res; |
| 329 | 332 |
} |
| ... | ... | |
| 353 | 356 |
|
| 354 | 357 |
#ifndef NDEBUG |
| 355 | 358 |
if (res) {
|
| 356 |
chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: i2c transmit failed (%d)\n", __FILE__ , __LINE__ , res);
|
|
| 359 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): ERROR: i2c transmit failed (%d)\n", __FILE__ , __LINE__ , res);
|
|
| 357 | 360 |
} |
| 358 | 361 |
#endif |
| 359 | 362 |
|
| ... | ... | |
| 369 | 372 |
{
|
| 370 | 373 |
if (!buf) {
|
| 371 | 374 |
#ifndef NDEBUG |
| 372 |
chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: received NULL-pointer as buffer\n", __FILE__ , __LINE__);
|
|
| 375 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): ERROR: received NULL-pointer as buffer\n", __FILE__ , __LINE__);
|
|
| 373 | 376 |
#endif |
| 374 | 377 |
return ~RDY_OK; |
| 375 | 378 |
} |
| 376 | 379 |
if (rw != EXT_CMD_WRITE && rw != EXT_CMD_READ) {
|
| 377 | 380 |
#ifndef NDEBUG |
| 378 |
chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: invalid access mode selected\n", __FILE__ , __LINE__);
|
|
| 381 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): ERROR: invalid access mode selected\n", __FILE__ , __LINE__);
|
|
| 379 | 382 |
#endif |
| 380 | 383 |
return ~RDY_OK; |
| 381 | 384 |
} |
| 382 | 385 |
if (length > 33) {
|
| 383 | 386 |
#ifndef NDEBUG |
| 384 |
chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: length exceeds maximum of 33 bytes\n", __FILE__ , __LINE__);
|
|
| 387 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): ERROR: length exceeds maximum of 33 bytes\n", __FILE__ , __LINE__);
|
|
| 385 | 388 |
#endif |
| 386 | 389 |
return ~RDY_OK; |
| 387 | 390 |
} |
| ... | ... | |
| 402 | 405 |
|
| 403 | 406 |
#ifndef NDEBUG |
| 404 | 407 |
if (res) {
|
| 405 |
chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: i2c transmit failed (%d)\n", __FILE__ , __LINE__ , res);
|
|
| 408 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): ERROR: i2c transmit failed (%d)\n", __FILE__ , __LINE__ , res);
|
|
| 406 | 409 |
} |
| 407 | 410 |
#endif |
| 408 | 411 |
|
| components/power/ina219.cpp | ||
|---|---|---|
| 2 | 2 |
|
| 3 | 3 |
#include <ch.hpp> |
| 4 | 4 |
#include <chprintf.h> |
| 5 |
#include <global.hpp> |
|
| 5 | 6 |
|
| 6 | 7 |
using namespace chibios_rt; |
| 7 | 8 |
using namespace amiro; |
| 8 | 9 |
using namespace INA219; |
| 9 | 10 |
|
| 11 |
extern Global global; |
|
| 12 |
|
|
| 10 | 13 |
Driver::Driver(I2CDriver &i2c_driver, const uint8_t i2c_address) : |
| 11 | 14 |
BaseSensor<InitData,CalibData>(), i2c_driver(&i2c_driver), tx_params({i2c_address, NULL, 0, NULL, 0}), current_lsb_uA(0)
|
| 12 | 15 |
{
|
| ... | ... | |
| 225 | 228 |
bus_voltage = this->readBusVoltage(); |
| 226 | 229 |
this->readRegister(REG_POWER, power); |
| 227 | 230 |
} |
| 228 |
chprintf((BaseSequentialStream*) &SD1, "shunt voltage : %fV\n", this->readShuntVoltage_uV() / 1000000.f);
|
|
| 229 |
chprintf((BaseSequentialStream*) &SD1, "bus voltage : %fV\n", bus_voltage.voltage_uV / 1000000.f);
|
|
| 230 |
chprintf((BaseSequentialStream*) &SD1, "power : %fW\n", this->readPower_uW() / 1000000.f);
|
|
| 231 |
chprintf((BaseSequentialStream*) &SD1, "current : %fA\n", this->readCurrent_uA() / 1000000.f);
|
|
| 231 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "shunt voltage : %fV\n", this->readShuntVoltage_uV() / 1000000.f);
|
|
| 232 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "bus voltage : %fV\n", bus_voltage.voltage_uV / 1000000.f);
|
|
| 233 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "power : %fW\n", this->readPower_uW() / 1000000.f);
|
|
| 234 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "current : %fA\n", this->readCurrent_uA() / 1000000.f);
|
|
| 232 | 235 |
|
| 233 | 236 |
return ST_OK; |
| 234 | 237 |
} |
| ... | ... | |
| 343 | 346 |
dst &= MASK_CALIBRATION; |
| 344 | 347 |
#ifndef NDEBUG |
| 345 | 348 |
} else {
|
| 346 |
chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: i2c transmit failed (%d | 0x%08X)\n", __FILE__ , __LINE__ , res, this->i2c_driver->getErrors());
|
|
| 349 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): ERROR: i2c transmit failed (%d | 0x%08X)\n", __FILE__ , __LINE__ , res, this->i2c_driver->getErrors());
|
|
| 347 | 350 |
#endif |
| 348 | 351 |
} |
| 349 | 352 |
|
| ... | ... | |
| 366 | 369 |
|
| 367 | 370 |
#ifndef NDEBUG |
| 368 | 371 |
if (res) {
|
| 369 |
chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: i2c transmit failed (%d | 0x%08X)\n", __FILE__ , __LINE__ , res, this->i2c_driver->getErrors());
|
|
| 372 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): ERROR: i2c transmit failed (%d | 0x%08X)\n", __FILE__ , __LINE__ , res, this->i2c_driver->getErrors());
|
|
| 370 | 373 |
} |
| 371 | 374 |
#endif |
| 372 | 375 |
|
Also available in: Unified diff