engine_control/source/application/collision_ctrl.c

/*
 * collision_ctrl.c
 *
 *  Created on: Aug 30, 2016
 *      Author: tkl
 */

#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>

#include "board_devices.h"
#include "queue.h"
#include "list.h"
#include "stack.h"
#include "kernel.h"
#include "driver.h"
#include "shell.h"
#include "drive_ctrl.h"
#include "engine_ctrl.h"

#pragma pack(push)
#pragma pack(1)
struct collision_ctrl_object {
	int distance_mm;
};
#pragma pack(pop)

struct collision_ctrl_object collision_ctrl_object;
struct thread_context collision_ctrl_ctx;
stack_t collision_ctrl_stack[256];

static void collision_ctrl_loop(void *arg)
{
	drv_open(&pwm5_c2);	/* trigger pwm */
	drv_open(&pwm2_c4); /* read pwm */
	drv_open(&rng);
	while(1) {
		int pulse_ns = drv_ioctl(&pwm2_c4, IOCTL_PWM_GET_PULSE_WIDTH_NS, NULL);
		collision_ctrl_object.distance_mm = 343 * pulse_ns / 1000 / 2;
		sleep_ms(1);
	}
}

int collision_ctrl_init(void)
{
	thread_create(
			&collision_ctrl_ctx,
			collision_ctrl_stack,
			sizeof(collision_ctrl_stack) / sizeof(stack_t),
			collision_ctrl_loop,
			NULL,
			THREAD_PRIO_LOW);

	return 0;
}

int collision_ctrl_get_distance_mm(void)
{
	return collision_ctrl_object.distance_mm;
}

bool collision_ctrl_is_collision(void)
{
	if(collision_ctrl_object.distance_mm < 400)
		return true;
	return false;
}