Skip to content

Commit

Permalink
Seem to read batt volt = zero some times, apparently we do (now) need…
Browse files Browse the repository at this point in the history
… to wait for ADC conv done..
  • Loading branch information
Damme committed Jul 7, 2024
1 parent fa0a053 commit 9122c97
Show file tree
Hide file tree
Showing 2 changed files with 11 additions and 5 deletions.
5 changes: 3 additions & 2 deletions motorctrl.c
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#include <stdio.h>
#include "common.h"
#include "motorctrl.h"
#include "timers.h"
#include <stdio.h>

int16_t setPWMBlade, setPWMLeft, setPWMRight = 0;
int16_t curPWMBlade, curPWMLeft, curPWMRight = 0;
Expand Down Expand Up @@ -41,7 +42,7 @@ void motionSensor_Timer(TimerHandle_t xTimer) {
if (sensorMsg.accelZ < 5000 && sensorMsg.currentPWMBlade > 0) {
tiltCounter++;
if (tiltCounter > 500) {
debug(" TILT EMG STOP");
//debug(" TILT EMG STOP");
xMotorMsgType MotorMsg;
MotorMsg.action = MOTORREQ_EMGSTOP;
MotorMsg.pwm.left = 0;
Expand Down
11 changes: 8 additions & 3 deletions sensor.c
Original file line number Diff line number Diff line change
Expand Up @@ -147,12 +147,15 @@ void sensor_Task(void *pvParameters) {
#define SAMPLES 2000

for (int i=0; i < SAMPLES; i++) {
I2C1_Recv_Addr_Buf(L3GD20, 0x28 | (1 << 7), 1, sizeof(gyro), (uint8_t*)&gyro); // Bug? seem to freeze sensor task? i2c code!
I2C1_Recv_Addr_Buf(L3GD20, 0x28 | (1 << 7), 1, sizeof(gyro), (uint8_t*)&gyro);
rawYaw += (int16_t)((gyro.X1 << 8) | gyro.X2);
rawPitch += (int16_t)((gyro.Y1 << 8) | gyro.Y2);
rawRoll += (int16_t)((gyro.Z1 << 8) | gyro.Z2);
vTaskDelay(pdMS_TO_TICKS(5));
}

while (!(LPC_ADC->GDR & (1<<31))); // Wait for ADC conv. Done

offsetRoll = imu_temp_offset_z(conv_board_temp(ADC_DR_RESULT(ANALOG_BOARD_TEMP)));
rawYaw = round(rawYaw / SAMPLES);
rawPitch = round(rawPitch / SAMPLES);
Expand Down Expand Up @@ -195,6 +198,8 @@ void sensor_Task(void *pvParameters) {
sensorMsg.accelY *= GRAVITY;
sensorMsg.accelZ *= GRAVITY;

while (!(LPC_ADC->GDR & (1<<31))); // Wait for ADC conv. Done

// L3GD20
I2C1_Recv_Addr_Buf(L3GD20, 0x28 | (1 << 7), 1, sizeof(gyro), (uint8_t*)&gyro);
offsetRoll = imu_temp_offset_z(conv_board_temp(ADC_DR_RESULT(ANALOG_BOARD_TEMP)));
Expand All @@ -217,8 +222,8 @@ void sensor_Task(void *pvParameters) {
xQueueSend(RosTxQueue, local_txbuf, xDelay10);
cJSON_Delete(root);

// Is this really necessary? test to disable wait for adc done to test stability
//while (!(LPC_ADC->GDR & (1<<31))); // Wait for ADC conv. Done


sensorMsg.batteryTemp = conv_batt_temp(ADC_DR_RESULT(ANALOG_BATT_TEMP));

ema_batteryVolt = ((ALPHA * (ADC_DR_RESULT(ANALOG_BATT_VOLT) * 100000 / 13068)) + ((SCALE - ALPHA) * ema_batteryVolt)) / SCALE;
Expand Down

0 comments on commit 9122c97

Please sign in to comment.