Skip to content

GitLab

Commit d7df11f7 authored by mcand001's avatar mcand001
Browse files

Cleaned up the code

parent 79a3c27b
Pipeline #10004 passed with stages
in 2 minutes and 59 seconds
Hide whitespace changes
Inline Side-by-side
src/controller/controller_blinkerTask.c
View file @ d7df11f7
......@@ -7,10 +7,12 @@ void controller_blinkerTask(void *data) {
int32_t ret;
uint32_t status;
TickType_t lastWakeTime = xTaskGetTickCount();
for (;;) {
/* Check Blinker is not active */
ret = shm_read32(ctrl->shm, SHM_CONTROLL_CTRL, &status);
CONFIG_ASSERT(ret >= 0);
if (!(status & (SHM_CONTROLL_CTRL_BLINK_L | SHM_CONTROLL_CTRL_BLINK_R))) {
/* Clear Pin and suspend */
ret = gpioPin_clearPin(ctrl->blinkerL);
......@@ -32,6 +34,7 @@ void controller_blinkerTask(void *data) {
/* reset time */
lastWakeTime = xTaskGetTickCount();
}
if (status & SHM_CONTROLL_CTRL_BLINK_L) {
ret = gpioPin_togglePin(ctrl->blinkerL);
CONFIG_ASSERT(ret >= 0);
......@@ -39,6 +42,7 @@ void controller_blinkerTask(void *data) {
ret = gpioPin_clearPin(ctrl->blinkerL);
CONFIG_ASSERT(ret >= 0);
}
if (status & SHM_CONTROLL_CTRL_BLINK_R) {
ret = gpioPin_togglePin(ctrl->blinkerR);
CONFIG_ASSERT(ret >= 0);
......
src/controller/controller_btn.c
View file @ d7df11f7
......@@ -6,7 +6,6 @@ bool btn_pairs_callback(struct gpio_pin *pin, uint32_t pinID, void *data) {
BaseType_t isTaskWoken;
struct queue_data *queueData = data;
isTaskWoken = pdFALSE;
/* Workaround for Hardware Bug
* If the camera is working/running, the button callback handler
* will be fired on falling and rising flanks for gpio pins.
......@@ -21,11 +20,13 @@ bool btn_pairs_callback(struct gpio_pin *pin, uint32_t pinID, void *data) {
if (isTaskWoken) {
return true;
}
return false;
}
static void display_btn_led(struct controller *ctrl) {
uint32_t status = controller_getStatus(ctrl, 10 / portTICK_PERIOD_MS);
if (status & SHM_TELEMETRIE_STATUS_FIRST_BTN) {
gpioPin_setPin(ctrl->firstBtnPair[1]);
} else {
......@@ -55,8 +56,8 @@ void controller_btnPressTask(void *data) {
for (;;) {
result = xQueueReceive(ctrl->btnPressQueue, &id, portMAX_DELAY);
if (result) {
if (result) {
if (id == PARK_SENSOR) {
bool value = gpioPin_getValue(ctrl->parkSensor);
status = SHM_TELEMETRIE_STATUS_PARK_SENSOR;
......@@ -88,9 +89,7 @@ void controller_btnPressTask(void *data) {
break;
}
status_result = controller_toggleStatusBits(ctrl, status,
100 / portTICK_PERIOD_MS);
status_result = controller_toggleStatusBits(ctrl, status, 100 / portTICK_PERIOD_MS);
CONFIG_ASSERT(status_result >= 0);
display_btn_led(ctrl);
}
......
src/controller/controller_deinit.c
View file @ d7df11f7
......@@ -5,12 +5,14 @@
int32_t controller_deinit(struct controller *ctrl) {
motor_set(ctrl->motor, 0);
motor_set(ctrl->servo, 0);
#ifdef CONFIG_PID_CONTROLLER
pid_deinit(ctrl->pid);
#endif
#ifdef CONFIG_INCLUDE_vTaskDelete
vTaskDelete(ctrl->btnPressTask);
#endif
gpioPin_deinit(ctrl->firstBtnPair[0]);
gpioPin_deinit(ctrl->secondBtnPair[0]);
gpioPin_deinit(ctrl->thirdBtnPair[0]);
......@@ -20,6 +22,7 @@ int32_t controller_deinit(struct controller *ctrl) {
gpioPin_deinit(ctrl->thirdBtnPair[1]);
vQueueDelete(ctrl->btnPressQueue);
return 0;
}
src/controller/controller_drive.c
View file @ d7df11f7
......@@ -6,23 +6,28 @@ void controller_handleDrive(struct controller *ctrl) {
uint32_t time;
int32_t ret;
uint32_t tmp = rc_get(ctrl->rc, 1);
if (tmp > 0) {
/* RC is active change to RC */
controller_activateRC(ctrl);
return;
}
ret = shm_read32(ctrl->shm, SHM_CONTROLL_CTRL, &tmp);
CONFIG_ASSERT(ret >= 0);
/* if drive not set shutdown */
if (!(tmp & SHM_CONTROLL_CTRL_DRIVE)) {
controller_shutdown(ctrl);
return;
}
#ifdef CONFIG_INCLUDE_vTaskSuspend
if (tmp & (SHM_CONTROLL_CTRL_BLINK_L | SHM_CONTROLL_CTRL_BLINK_R)) {
vTaskResume(ctrl->blinkerTask);
}
#endif
ret = shm_read32(ctrl->shm, SHM_CONTROLL_UPDATE_TIME, &time);
CONFIG_ASSERT(ret >= 0);
if (time != ctrl->lasttimeLinux) {
......@@ -45,6 +50,7 @@ void controller_handleDrive(struct controller *ctrl) {
emergency_shutdown();
#endif
}
if (-30 > throttle || throttle > 30) {
printf("throttle: %f out of range[-30,30]\n", throttle);
/* Illegal Data Shutdwon */
......@@ -57,6 +63,7 @@ void controller_handleDrive(struct controller *ctrl) {
steering == 1500) {
ctrl->first = false;
}
if (!ctrl->first) {
/* set the desired values for the main loop */
ctrl->desired_throttle = throttle;
......@@ -65,6 +72,7 @@ void controller_handleDrive(struct controller *ctrl) {
#ifndef CONFIG_PID_CONTROLLER
controller_updateDriveParams(ctrl, ctrl->desired_throttle, ctrl->desired_steering);
if (throttle > 0.0000001) {
motor_set(ctrl->motor,
......@@ -77,18 +85,21 @@ void controller_handleDrive(struct controller *ctrl) {
motor_set(ctrl->motor,
CONTROLLER_THROTTLE_ZERO);
}
motor_set(ctrl->servo, steering);
#endif /* ifdef CONFIG_PID_CONTROLLER */
}
} else {
uint32_t diff;
time = xTaskGetTickCount();
if (ctrl->lasttime <= time) {
diff = time - ctrl->lasttime;
} else {
/* Overflow */
diff = (portMAX_DELAY - ctrl->lasttime) + time;
}
/* timeout */
if (diff > CONFIG_CONTROLLER_DRIVE_TIMEOUT) {
printf("[%lu] Control Timeout: last: %lu linux time: %lu\n",
......@@ -106,6 +117,7 @@ void controller_handleDrive(struct controller *ctrl) {
int32_t controller_activateDrive(struct controller *ctrl) {
HAL_LOCK(ctrl, 20 / portTICK_PERIOD_MS, -1);
/* ignore command if emergency is set */
if (!(ctrl->status & SHM_TELEMETRIE_STATUS_EMERGENCY)) {
printf("Activate Linux Drive\n");
......@@ -115,6 +127,7 @@ int32_t controller_activateDrive(struct controller *ctrl) {
ctrl->status &= ~SHM_TELEMETRIE_STATUS_RC;
ctrl->status |= SHM_TELEMETRIE_STATUS_DRIVE;
}
HAL_UNLOCK(ctrl, -1);
return 0;
}
src/controller/controller_emergency.c
View file @ d7df11f7
......@@ -3,8 +3,10 @@
int32_t controller_emergency(struct controller *ctrl) {
printf("Emergency Shutdown\n");
ctrl->status |= SHM_TELEMETRIE_STATUS_EMERGENCY;
gpioPin_setPin(ctrl->bmsLED);
controller_shutdown(ctrl);
return 0;
}
src/controller/controller_init.c
View file @ d7df11f7
......@@ -14,26 +14,30 @@
#include "controller_prv.h"
struct controller controller0 = {.init = false};
struct controller *controller_init(struct motorcontroller *motorcontroller,
struct counter *mc_counter,
struct speedsensor *mc_speedsensor,
struct motor *motor,
struct motor *servo,
struct rc *rc,
struct shm *shm,
struct counter **wheel_counter,
struct speedsensor **wheel_speedsensor,
struct gpio_pin *rcLED,
struct gpio_pin *bmsLED,
struct gpio_pin *blinkerL,
struct gpio_pin *blinkerR,
struct gpio_pin *brake,
struct gpio_pin *firstBtnPair[2],
struct gpio_pin *secondBtnPair[2],
struct gpio_pin *thirdBtnPair[2],
struct gpio_pin *parkSensor) {
struct controller *controller_init(
struct motorcontroller *motorcontroller,
struct counter *mc_counter,
struct speedsensor *mc_speedsensor,
struct motor *motor,
struct motor *servo,
struct rc *rc,
struct shm *shm,
struct counter **wheel_counter,
struct speedsensor **wheel_speedsensor,
struct gpio_pin *rcLED,
struct gpio_pin *bmsLED,
struct gpio_pin *blinkerL,
struct gpio_pin *blinkerR,
struct gpio_pin *brake,
struct gpio_pin *firstBtnPair[2],
struct gpio_pin *secondBtnPair[2],
struct gpio_pin *thirdBtnPair[2],
struct gpio_pin *parkSensor) {
/* Beginning of the method */
struct controller *ctrl = &controller0;
int32_t ret;
if (ctrl->init) {
return ctrl;
}
......@@ -131,7 +135,6 @@ struct controller *controller_init(struct motorcontroller *motorcontroller,
queueData.id = PARK_SENSOR;
ctrl->parkSensorArg = queueData;
ctrl->firstBtnPair = firstBtnPair;
ctrl->secondBtnPair = secondBtnPair;
ctrl->thirdBtnPair = thirdBtnPair;
......@@ -167,11 +170,13 @@ struct controller *controller_init(struct motorcontroller *motorcontroller,
ret |= gpioPin_enableInterrupt(secondBtnPair[0]);
ret |= gpioPin_enableInterrupt(thirdBtnPair[0]);
ret |= gpioPin_enableInterrupt(parkSensor);
if (ret != 0) {
goto controller_init_error1;
}
ctrl->lock = OS_CREATE_MUTEX_RECURSIVE(ctrl->lock);
if (!ctrl->lock) {
goto controller_init_error1;
}
......@@ -189,37 +194,26 @@ struct controller *controller_init(struct motorcontroller *motorcontroller,
goto controller_init_error1;
}
/* FreeROTS Task are not contex of the proof */
ret = OS_CREATE_TASK(controller_rcLEDTask,
"Contoller LED Task",
250,
ctrl,
2,
ctrl->rcLEDTask);
ret = OS_CREATE_TASK(controller_rcLEDTask, "Contoller LED Task", 250, ctrl, 2, ctrl->rcLEDTask);
if (ret != pdPASS) {
goto controller_init_error1;
}
ret = OS_CREATE_TASK(controller_blinkerTask,
"Blinker LED Task",
250,
ctrl,
2,
ctrl->blinkerTask);
ret = OS_CREATE_TASK(controller_blinkerTask, "Blinker LED Task", 250, ctrl, 2, ctrl->blinkerTask);
if (ret != pdPASS) {
goto controller_init_error1;
}
ret = OS_CREATE_TASK(controller_btnPressTask,
"GPIO Button Press Task",
250,
ctrl,
2,
ctrl->btnPressTask);
ret = OS_CREATE_TASK(controller_btnPressTask, "GPIO Button Press Task", 250, ctrl, 2, ctrl->btnPressTask);
if (ret != pdPASS) {
goto controller_init_error1;
}
return ctrl;
controller_init_error1:
motor_set(ctrl->motor, 0);
motor_set(ctrl->servo, 0);
......
src/controller/controller_rc.c
View file @ d7df11f7
......@@ -25,10 +25,12 @@ void controller_handleRC(struct controller *ctrl) {
controller_shutdown(ctrl);
return;
}
/* first throttle shall be ~1500 */
if (ctrl->first && (throttle >= CONTROLLER_RC_MIN && throttle <= CONTROLLER_RC_MAX)) {
ctrl->first = false;
}
if (!ctrl->first) {
#ifdef CONFIG_PID_CONTROLLER
/* make a small room in that nothing happens */
......@@ -41,7 +43,6 @@ void controller_handleRC(struct controller *ctrl) {
/* set the desired value for the main loop */
ctrl->desired_steering = steering;
#else
ctrl->desired_throttle = throttle;
ctrl->desired_steering = steering;
#endif
......@@ -55,6 +56,7 @@ void controller_handleRC(struct controller *ctrl) {
/* write the rc values for pid_tuning */
ret = shm_write32(ctrl->shm, SHM_TELEMETRIE_RC_STEERING, ctrl->desired_steering);
CONFIG_ASSERT(ret >= 0);
#ifdef CONFIG_PID_CONTROLLER
ret = shm_writeFloat(ctrl->shm, SHM_TELEMETRIE_RC_THROTTLE, ctrl->desired_throttle);
CONFIG_ASSERT(ret >= 0);
......@@ -62,7 +64,6 @@ void controller_handleRC(struct controller *ctrl) {
ret = shm_writeFloat(ctrl->shm, SHM_TELEMETRIE_RC_THROTTLE, (float) throttle);
CONFIG_ASSERT(ret >= 0);
#endif
}
}
......@@ -81,5 +82,6 @@ int32_t controller_activateRC(struct controller *ctrl) {
#endif
}
HAL_UNLOCK(ctrl, -1);
return 0;
}
src/controller/controller_rcLEDTask.c
View file @ d7df11f7
......@@ -7,9 +7,11 @@ void controller_rcLEDTask(void *data) {
int32_t ret;
uint32_t status;
TickType_t lastWakeTime = xTaskGetTickCount();
for (;;) {
/* Check RC not active */
status = controller_getStatus(ctrl, 500 / portTICK_PERIOD_MS);
if (!(status & SHM_TELEMETRIE_STATUS_RC)) {
/* Clear Pin and suspend */
ret = gpioPin_clearPin(ctrl->rcLED);
......@@ -23,8 +25,7 @@ void controller_rcLEDTask(void *data) {
status = controller_getStatus(ctrl, 500 / portTICK_PERIOD_MS);
vTaskDelayUntil(&lastWakeTime, 500 / portTICK_PERIOD_MS);
} while (!(status & SHM_TELEMETRIE_STATUS_RC));
#endif /*
* ifdef CONFIG_INCLUDE_vTaskSuspend
#endif /* ifdef CONFIG_INCLUDE_vTaskSuspend
* reset time */
lastWakeTime = xTaskGetTickCount();
}
......
src/controller/controller_shutdown.c
View file @ d7df11f7
......@@ -43,9 +43,7 @@ int32_t controller_shutdown(struct controller *ctrl) {
ctrl->status &= ~(SHM_TELEMETRIE_STATUS_RC | SHM_TELEMETRIE_STATUS_DRIVE);
status = ctrl->status;
} else {
ret = controller_setAndClearStatusBits(ctrl, SHM_TELEMETRIE_STATUS_RC |
SHM_TELEMETRIE_STATUS_DRIVE, 0, 20 /
portTICK_PERIOD_MS);
ret = controller_setAndClearStatusBits(ctrl, SHM_TELEMETRIE_STATUS_RC | SHM_TELEMETRIE_STATUS_DRIVE, 0, 20 / portTICK_PERIOD_MS);
CONFIG_ASSERT(ret >= 0);
status = controller_getStatus(ctrl, 20 / portTICK_PERIOD_MS);
}
......@@ -56,5 +54,6 @@ int32_t controller_shutdown(struct controller *ctrl) {
/* update shm*/
ret = shm_write32(ctrl->shm, SHM_TELEMETRIE_STATUS, status);
CONFIG_ASSERT(ret >= 0);
return 0;
}
src/controller/controller_stateMaschine.c
View file @ d7df11f7
......@@ -56,8 +56,7 @@ void controller_updateStateMaschine(struct controller *ctrl, int32_t throttle) {
}
statusClear |=
(SHM_TELEMETRIE_STATUS_FORWARD | SHM_TELEMETRIE_STATUS_NOT_MOVEING |
SHM_TELEMETRIE_STATUS_REVERSE);
(SHM_TELEMETRIE_STATUS_FORWARD | SHM_TELEMETRIE_STATUS_NOT_MOVEING | SHM_TELEMETRIE_STATUS_REVERSE);
switch (ctrl->dir) {
case CONTROLLER_FORWARD:
......@@ -71,8 +70,7 @@ void controller_updateStateMaschine(struct controller *ctrl, int32_t throttle) {
break;
}
ret = controller_setAndClearStatusBits(ctrl, statusClear, statusSet, 20 /
portTICK_PERIOD_MS);
ret = controller_setAndClearStatusBits(ctrl, statusClear, statusSet, 20 / portTICK_PERIOD_MS);
CONFIG_ASSERT(ret >= 0);
}
src/controller/controller_task_framaC.c
View file @ d7df11f7
......@@ -73,9 +73,7 @@ static float car_is_moving(struct controller *ctrl) {
speed_f = 0;
}
}
/**
* Don't update speed if we not moving
*/
/* Don't update speed if we not moving */
if (speed_f < -0.001 || speed_f > 0.001) {
time = ((dist / speed_f) * 1000) / (10. * 2);
framaC_capture_setValue(wheel_capture[0], time);
......@@ -124,6 +122,7 @@ void contoller_simulation(struct controller *ctrl) {
drive_steering = 500;
}
break;
case TEST_DRIVE_IN_DRIVE_MODE_FIRST:
/* Now we are in Drive Mode we can set Throttle */
{
......@@ -137,6 +136,7 @@ void contoller_simulation(struct controller *ctrl) {
}
}
break;
case TEST_DRIVE_IN_DRIVE_MODE:
{
/* Check Status Bit Mask */
......@@ -152,6 +152,7 @@ void contoller_simulation(struct controller *ctrl) {
drive_steering = 600;
}
break;
case TEST_SWITCH_RC_MODE:
{
/* Check Status Bit Mask */
......@@ -168,12 +169,14 @@ void contoller_simulation(struct controller *ctrl) {
rc_steering = 1400;
}
break;
case TEST_DRIVE_IN_RC_MODE_FIRST:
{
rc_throttle = 1500;
rc_steering = 1500;
}
break;
case TEST_DRIVE_IN_RC_MODE:
{
/* Check Status Bit Mask */
......@@ -189,6 +192,7 @@ void contoller_simulation(struct controller *ctrl) {
rc_steering = 1500;
}
break;
case TEST_SWITCH_BACK_TO_DRIVE_MODE:
{
/* Check Status Bit Mask */
......@@ -219,6 +223,7 @@ void contoller_simulation(struct controller *ctrl) {
drive_steering = 500;
}
break;
case TEST_DRIVE_IN_DRIVE_MODE_2:
{
/* Check Status Bit Mask */
......@@ -231,6 +236,7 @@ void contoller_simulation(struct controller *ctrl) {
}
}
break;
case TEST_SET_PID_SETTINGS:
{
/* only update (settings are stored at init so no values are changed */
......@@ -239,6 +245,7 @@ void contoller_simulation(struct controller *ctrl) {
}
break;
case TEST_DRIVE_TIMEOUT:
{
/* Check Status Bit Mask */
......@@ -252,6 +259,7 @@ void contoller_simulation(struct controller *ctrl) {
driveUpdateTime = false;
}
break;
case TEST_DRIVE_TIMEOUT_END:
{
/* Check Status Bit Mask */
......@@ -265,6 +273,7 @@ void contoller_simulation(struct controller *ctrl) {
driveUpdateTime = true;
}
break;
case TEST_SHUTDOWN:
{
/* Check Status Bit Mask */
......@@ -278,6 +287,7 @@ void contoller_simulation(struct controller *ctrl) {
drive = false;
}
break;
case (SIZEOF_TESTS - 1):
{
{
......@@ -290,6 +300,7 @@ void contoller_simulation(struct controller *ctrl) {
}
break;
}
{
static bool oldDrive = false;
static bool oldDriveUpdateTime = true;
......@@ -328,6 +339,7 @@ void contoller_simulation(struct controller *ctrl) {
rc_simulation(timer, capture[1], &timer_steering, rc_steering);
rc_simulation(timer, capture[1], &timer_steering, (20000 - rc_steering));
} else {
/* deactivate RC to switch back to Drive Mode */#endif
framaC_timer_overflow(timer);
......@@ -346,25 +358,9 @@ void controller_task_init() {
struct controller *ctrl;
int32_t ret;
hw = hartwareInit();
ctrl = controller_init(
hw->motorcontroller,
hw->mc_counter,
hw->mc_speedsensor,
hw->motor,
hw->servo,
hw->rc,
hw->shm,
hw->wheel_counter,
hw->wheel_speedsensor,
hw->rcLED,
hw->bmsLED,
hw->blinkerL,
hw->blinkerR,
hw->brake,
hw->firstBtnPair,
hw->secondBtnPair,
hw->thirdBtnPair,
hw->parkSensor);
ctrl = controller_init(hw->motorcontroller, hw->mc_counter, hw->mc_speedsensor, hw->motor, hw->servo, hw->rc,
hw->shm, hw->wheel_counter, hw->wheel_speedsensor, hw->rcLED, hw->bmsLED, hw->blinkerL,
hw->blinkerR, hw->brake, hw->firstBtnPair, hw->secondBtnPair, hw->thirdBtnPair, hw->parkSensor);
/*@ assert ctrl != NULL; */
ctrl->running = false;
//controller_task(ctrl);
......
src/controller/controller_throttle.c
View file @ d7df11f7
......@@ -15,7 +15,8 @@ int32_t controller_motor_throttle_set(struct controller *ctrl, float setpoint) {
float d;
float pid_scale;
float pid_offset;
float actual_value; /* the actual speed in m/s */
/* the actual speed in m/s */
float actual_value;
/* if the updatetime has changed*/
ret = shm_read32(ctrl->shm, SHM_CONTROLL_PID_UPDATE_TIME, &time);
......@@ -23,6 +24,7 @@ int32_t controller_motor_throttle_set(struct controller *ctrl, float setpoint) {
if (time != ctrl->lasttimePID) {
/* Save the update time*/
ctrl->lasttimePID = time;
/*get new pid params*/
ret = shm_readFloat(ctrl->shm, SHM_CONTROLL_P_GAIN, &p);
CONFIG_ASSERT(ret >= 0);
......@@ -30,8 +32,10 @@ int32_t controller_motor_throttle_set(struct controller *ctrl, float setpoint) {
CONFIG_ASSERT(ret >= 0);
ret = shm_readFloat(ctrl->shm, SHM_CONTROLL_D_GAIN, &d);
CONFIG_ASSERT(ret >= 0);
/*update the pid params and reset*/
pid_set_gains(ctrl->pid, p, i, d);
/* Get the recalculation params */
ret = shm_readFloat(ctrl->shm, SHM_CONTROLL_PID_SCALE, &pid_scale);
CONFIG_ASSERT(ret >= 0);
......@@ -39,13 +43,8 @@ int32_t controller_motor_throttle_set(struct controller *ctrl, float setpoint) {
CONFIG_ASSERT(ret >= 0);
ctrl->pid_scale = pid_scale;
ctrl->pid_offset = pid_offset;
printf("Update PID Settings: p: %f i: %f d: %f scale: %f offset: %f\n",
p,
i,
d,
pid_scale,
pid_offset);
printf("Update PID Settings: p: %f i: %f d: %f scale: %f offset: %f\n", p, i, d, pid_scale, pid_offset);
}
/* get the actual_value*/
......
src/controller/controller_updateDriveParams.c
View file @ d7df11f7
......@@ -3,9 +3,7 @@
#include <speedsensor.h>
#include <math.h>
/**
* calculates SHM_TELEMETRIE_SPEED_CMS, SHM_TELEMETRIE_DISTANCE and detects the direction
*/
/* calculates SHM_TELEMETRIE_SPEED_CMS, SHM_TELEMETRIE_DISTANCE and detects the direction */
void controller_updateDriveParams(struct controller *ctrl, float throttle, uint32_t streering) {
#ifdef CONFIG_CAR_MOTOR_MOTORCONTROLLER
int32_t ret;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment