Commit 8dcf9728 by Elvar Liiv

a

parent f147a242
Showing with 4264 additions and 0 deletions
This source diff could not be displayed because it is too large. You can view the blob instead.
/*
FreeRTOS V9.0.0 - Copyright (C) 2016 Real Time Engineers Ltd.
All rights reserved
VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION.
This file is part of the FreeRTOS distribution.
FreeRTOS is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License (version 2) as published by the
Free Software Foundation >>!AND MODIFIED BY!<< the FreeRTOS exception.
***************************************************************************
>>! NOTE: The modification to the GPL is included to allow you to !<<
>>! distribute a combined work that includes FreeRTOS without being !<<
>>! obliged to provide the source code for proprietary components !<<
>>! outside of the FreeRTOS kernel. !<<
***************************************************************************
FreeRTOS is distributed in the hope that it will be useful, but WITHOUT ANY
WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. Full license text is available on the following
link: http://www.freertos.org/a00114.html
***************************************************************************
* *
* FreeRTOS provides completely free yet professionally developed, *
* robust, strictly quality controlled, supported, and cross *
* platform software that is more than just the market leader, it *
* is the industry's de facto standard. *
* *
* Help yourself get started quickly while simultaneously helping *
* to support the FreeRTOS project by purchasing a FreeRTOS *
* tutorial book, reference manual, or both: *
* http://www.FreeRTOS.org/Documentation *
* *
***************************************************************************
http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading
the FAQ page "My application does not run, what could be wrong?". Have you
defined configASSERT()?
http://www.FreeRTOS.org/support - In return for receiving this top quality
embedded software for free we request you assist our global community by
participating in the support forum.
http://www.FreeRTOS.org/training - Investing in training allows your team to
be as productive as possible as early as possible. Now you can receive
FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers
Ltd, and the world's leading authority on the world's leading RTOS.
http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products,
including FreeRTOS+Trace - an indispensable productivity tool, a DOS
compatible FAT file system, and our tiny thread aware UDP/IP stack.
http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate.
Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS.
http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High
Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS
licenses offer ticketed support, indemnification and commercial middleware.
http://www.SafeRTOS.com - High Integrity Systems also provide a safety
engineered and independently SIL3 certified version for use in safety and
mission critical applications that require provable dependability.
1 tab == 4 spaces!
*/
#ifndef FREERTOS_CONFIG_H
#define FREERTOS_CONFIG_H
/*-----------------------------------------------------------
* Application specific definitions.
*
* These definitions should be adjusted for your particular hardware and
* application requirements.
*
* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE
* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE.
*
* See http://www.freertos.org/a00110.html.
*----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* Section where include file can be added */
/* USER CODE END Includes */
/* Ensure stdint is only used by the compiler, and not the assembler. */
#if defined(__ICCARM__) || defined(__CC_ARM) || defined(__GNUC__)
#include <stdint.h>
#include "main.h"
extern uint32_t SystemCoreClock;
#endif
#define configUSE_PREEMPTION 1
#define configSUPPORT_STATIC_ALLOCATION 0
#define configSUPPORT_DYNAMIC_ALLOCATION 1
#define configUSE_IDLE_HOOK 0
#define configUSE_TICK_HOOK 0
#define configCPU_CLOCK_HZ ( SystemCoreClock )
#define configTICK_RATE_HZ ((TickType_t)1000)
#define configMAX_PRIORITIES ( 7 )
#define configMINIMAL_STACK_SIZE ((uint16_t)128)
#define configTOTAL_HEAP_SIZE ((size_t)15360)
#define configMAX_TASK_NAME_LEN ( 16 )
#define configUSE_16_BIT_TICKS 0
#define configUSE_MUTEXES 1
#define configQUEUE_REGISTRY_SIZE 8
#define configUSE_PORT_OPTIMISED_TASK_SELECTION 1
/* Co-routine definitions. */
#define configUSE_CO_ROUTINES 0
#define configMAX_CO_ROUTINE_PRIORITIES ( 2 )
/* Set the following definitions to 1 to include the API function, or zero
to exclude the API function. */
#define INCLUDE_vTaskPrioritySet 1
#define INCLUDE_uxTaskPriorityGet 1
#define INCLUDE_vTaskDelete 1
#define INCLUDE_vTaskCleanUpResources 0
#define INCLUDE_vTaskSuspend 1
#define INCLUDE_vTaskDelayUntil 0
#define INCLUDE_vTaskDelay 1
#define INCLUDE_xTaskGetSchedulerState 1
/* Cortex-M specific definitions. */
#ifdef __NVIC_PRIO_BITS
/* __BVIC_PRIO_BITS will be specified when CMSIS is being used. */
#define configPRIO_BITS __NVIC_PRIO_BITS
#else
#define configPRIO_BITS 4
#endif
/* The lowest interrupt priority that can be used in a call to a "set priority"
function. */
#define configLIBRARY_LOWEST_INTERRUPT_PRIORITY 15
/* The highest interrupt priority that can be used by any interrupt service
routine that makes calls to interrupt safe FreeRTOS API functions. DO NOT CALL
INTERRUPT SAFE FREERTOS API FUNCTIONS FROM ANY INTERRUPT THAT HAS A HIGHER
PRIORITY THAN THIS! (higher priorities are lower numeric values. */
#define configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY 5
/* Interrupt priorities used by the kernel port layer itself. These are generic
to all Cortex-M ports, and do not rely on any particular library functions. */
#define configKERNEL_INTERRUPT_PRIORITY ( configLIBRARY_LOWEST_INTERRUPT_PRIORITY << (8 - configPRIO_BITS) )
/* !!!! configMAX_SYSCALL_INTERRUPT_PRIORITY must not be set to zero !!!!
See http://www.FreeRTOS.org/RTOS-Cortex-M3-M4.html. */
#define configMAX_SYSCALL_INTERRUPT_PRIORITY ( configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY << (8 - configPRIO_BITS) )
/* Normal assert() semantics without relying on the provision of an assert.h
header file. */
/* USER CODE BEGIN 1 */
#define configASSERT( x ) if ((x) == 0) {taskDISABLE_INTERRUPTS(); for( ;; );}
/* USER CODE END 1 */
/* Definitions that map the FreeRTOS port interrupt handlers to their CMSIS
standard names. */
#define vPortSVCHandler SVC_Handler
#define xPortPendSVHandler PendSV_Handler
/* IMPORTANT: This define MUST be commented when used with STM32Cube firmware,
to prevent overwriting SysTick_Handler defined within STM32Cube HAL */
/* #define xPortSysTickHandler SysTick_Handler */
/* USER CODE BEGIN Defines */
/* Section where parameter definitions can be added (for instance, to override default ones in FreeRTOS.h) */
/* USER CODE END Defines */
#endif /* FREERTOS_CONFIG_H */
/**
******************************************************************************
* File Name : ADC.h
* Description : This file provides code for the configuration
* of the ADC instances.
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2018 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __adc_H
#define __adc_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include "main.h"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
extern ADC_HandleTypeDef hadc1;
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
extern void _Error_Handler(char *, int);
void MX_ADC1_Init(void);
/* USER CODE BEGIN Prototypes */
/* USER CODE END Prototypes */
#ifdef __cplusplus
}
#endif
#endif /*__ adc_H */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
/**
******************************************************************************
* File Name : CAN.h
* Description : This file provides code for the configuration
* of the CAN instances.
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2018 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __can_H
#define __can_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include "main.h"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
extern void _Error_Handler(char *, int);
void MX_CAN1_Init(void);
void MX_CAN2_Init(void);
/* USER CODE BEGIN Prototypes */
void HAL_CAN_RxCpltCallback(CAN_HandleTypeDef* CanHandle);
void CAN1ConfigFilters(void);
void CAN2ConfigFilters(void);
/* USER CODE END Prototypes */
#ifdef __cplusplus
}
#endif
#endif /*__ can_H */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
#ifndef __CANID_H
#define __CANID_H
#define CAN_ID_READY 0x100 // Freq: ?? Len: ???
#define CAN_ID_KEY 0x101 // Freq: 10 Len: 1
#define CAN_ID_FRONT_LEFT_WHEEL 0x200 // Freq: 50 Len: 8
#define CAN_ID_BREAK 0x208 // Freq: 50 Len: 8
#define CAN_ID_GAS 0x210 // Freq: 50 Len: 8
#define CAN_ID_REAR_WHEELS 0x215 // Freq: 50 Len: 8
#define CAN_ID_BRAKE_PRESSED 0x231 // Freq: 50 Len: 8
#define CAN_ID_STEERING_WHEEL 0x236 // Freq: 10 Len: 8
#define CAN_ID_CHARGER_TEMP 0x286 // Freq: 10 Len: 8
#define CAN_ID_MOTOR_TEMP_RPM 0x298 // Freq: 10 Len: 8
#define CAN_ID_DRIVING_RANGE 0x346 // Freq: 50 Len: 8
#define CAN_ID_AMPS_VOLTS 0x373 // Freq: 100 Len: 8
#define CAN_ID_CHARGING_STATUS 0x374 // Freq: 10 Len: 8
#define CAN_ID_AC_AMPS_VOLTS 0x389 // Freq: 10 Len: 8
#define CAN_ID_AC_STATUS 0x3A4 // Freq: 10 Len: 8
#define CAN_ID_ODOMETER 0x412 // Freq: 10 Len: 8
#define CAN_ID_GEARBOX 0x418 // Freq: 20 Len: 7
#define CAN_ID_DOOR_WINDOW 0x424 // Freq: 25 Len: 8
#define CAN_ID_CELLINFO1 0x6E1 // Freq: 25 Len: 8
#define CAN_ID_CELLINFO2 0x6E2 // Freq: 25 Len: 8
#define CAN_ID_CELLINFO3 0x6E3 // Freq: 25 Len: 8
#define CAN_ID_CELLINFO4 0x6E4 // Freq: 25 Len: 8
// Limits
#define MIN_ST_WHEEL_ANGLE -0.89F
#define MAX_ST_WHEEL_ANGLE 0.89F // In radians; atan(wheelbase (2.55) / (turning circle (4.5) - car width(1.475))) = angle (40deg)
#define MIN_VELOCITY -5.5F
#define MAX_VELOCITY 5.5F // m/s ( => 19.8 km/h)
// Custom defined CAN messages
// Messages that come from ROS and forwarded
#define CAN_ID_ROS_VELOCITY 0x500
#define CAN_ID_ROS_SAFETY_BRAKE 0x520
#define CAN_ID_ROS_ST_WHEEL_ANGLE 0x540
// Messages from drive controller to master controller
#define CAN_ID_DRIVE_SPEED 0x510
#define CAN_ID_DRIVE_GAS_PEDAL 0x511
#define CAN_ID_DRIVE_SAFETY_BRAKE 0x530
#define CAN_ID_DRIVE_WHEEL_POSITION 0x550
#define CAN_ID_DRIVE_STEERING_PID 0x551
#define CAN_ID_DRIVE_GEARBOX 0x560
#define CAN_ID_DRIVE_HANDBRAKE 0x53F
//Debug messages
#define CAN_ID_DEBUG 0x321
//Custom defined messages lengths
#define CAN_ID_DRIVE_SPEED_LEN 8
#define CAN_ID_DRIVE_GAS_PEDAL_LEN 4
#define CAN_ID_DRIVE_SAFETY_BRAKE_LEN 2
#define CAN_ID_DRIVE_WHEEL_POSITION_LEN 7
#define CAN_ID_DRIVE_STEERING_PID_LEN 4
#define CAN_ID_DRIVE_GEARBOX_LEN 2
#define CAN_ID_DRIVE_HANDBRAKE_LEN 8
#endif /* __CANID_H */
/**
******************************************************************************
* File Name : DAC.h
* Description : This file provides code for the configuration
* of the DAC instances.
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2018 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __dac_H
#define __dac_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include "main.h"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
extern DAC_HandleTypeDef hdac;
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
extern void _Error_Handler(char *, int);
void MX_DAC_Init(void);
/* USER CODE BEGIN Prototypes */
/* USER CODE END Prototypes */
#ifdef __cplusplus
}
#endif
#endif /*__ dac_H */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
/*
* debug.h
*
* Created on: 11. april 2018
* Author: Karel Meriste
*/
#ifndef DEBUG_H_
#define DEBUG_H_
// Debug printout options
#define DBG_GAS_PEDAL 0
#define DBG_ST_WHEEL 0
#define DBG_GEAR 0
#define DBG_HANDBRAKE 0
#define DBG_ROS_VELOCITY 0
#define DBG_ROS_WHEEL 0
#define DBG_ROS_SAFETY_BRAKE 0
#endif /* DEBUG_H_ */
#include "stm32f4xx_hal.h"
#include "stm32f4xx_hal_can.h"
//GEARS
#define GEAR_P 1U //Park
#define GEAR_R 2U //Reverse
#define GEAR_N 3U //Neutral
#define GEAR_D 4U //Drive
#define GEAR_B 5U //
#define GEAR_C 6U //
#define GEAR_U 7U //Unknown
#define GEAR_E 8U //Error - None of values listed below (car ecu gear defs)
#define MIN_GEAR 1 //Minimum value for gearbox input
#define MAX_GEAR 4 //Maximum value for gearbox input. To use gears B and C, use values 5 or 6
//Car ECU gear definitions
#define ECU_GEAR_P 0x50 //Park
#define ECU_GEAR_R 0x52 //Reverse
#define ECU_GEAR_N 0x4E //Neutral
#define ECU_GEAR_D 0x44 //Drive
#define ECU_GEAR_C 0x83 //
#define ECU_GEAR_B 0x32 //
#define ECU_GEAR_U 0x20 //Unknown
//Gear change status reports
#define GEAR_CHANGE_WRONG_GEAR_ERROR 11
#define GEAR_CHANGE_READY 12
#define GEAR_CHANGE_ERROR 13
#define RPM_ODOMETER_NOT_ZERO_ERROR 14
#define RPM_NOT_ZERO_ERROR 15
#define ODOMETER_SPEED_NOT_ZERO_ERROR 16
//GAS PEDAL
#define MIN_GAS_PEDAL_DAC_VALUE 1263 // 1.00V value for STM32F4DISCOVERY DAC
#define MAX_GAS_PEDAL_DAC_VALUE 2300 // 2.91V is the maximum STM32F4DISCOVERT DAC
#define MAX_GAS_PEDAL_POS_VALUE 1000 //1393+1000 = 2393 <- ca 25% of car gas pedal max value
#define MAX_GAS_PEDAL_INTEGRAL 4095 // Should be same as max dac output value for gas pedal dac
#define MAX10 1700 // Max gas pedal dac output with speed less than 10 kmh
#define MAX15 2000 // Max gas pedal dac output with speed between 10 and 15 kmh
#define MAX20 2300 // Max gas pedal dac output with speed between 15 and 20 kmh
#define SPEED_10 10
#define SPEED_15 15
#define MAX_GAS_PEDAL_ALLOWED_DIFF 20 // Max allowed change in gas pedal dac output
//Gas pedal PID coefficients
#define GAS_PEDAL_KP 30 //Proportional coefficient for gas pedal PID
#define GAS_PEDAL_KI 0 //Integral coefficient for gas pedal PID
#define GAS_PEDAL_KD 400 //Derivative coefficient for gas pedal PID
//Car ECU
#define GAS_PEDAL_MAX_POS_ECU 0xB5 //Max gas pedal position from ECU
#define GAS_PEDAL_ERROR_VALUE_ECU 0xFF //Gas pedal error value from ECU
//BRAKES
//STEERING WHEEL
#define MIN_STEERING_WHEEL_DAC_OUTPUT 80000 // 0.50V (random value atm - depends on DAC to be used)
#define MAX_STEERING_WHEEL_DAC_OUTPUT 160000 // 4.50V (random value atm - depends on DAC to be used)
#define STEERING_WHEEL_MIDDLE 120000 // 2.5V value for steering wheel DAC (depends on DAC, must be changed)
//Steering wheel PID coeddicients
#define STEERING_WHEEL_KP 300 //Proportional coefficient for steering wheel PID
#define STEERING_WHEEL_KI 0 //Integral coefficient for steering wheel PID
#define STEERING_WHEEL_KD 0 //Derivative coefficient for steering wheel PID
#define STEERING_WHEEL_MAX_INTEGRAL 1250
//SAFETY BRAKES
#define SAFETY_BRAKE_ON 1
#define SAFETY_BRAKE_AUTONOMOUS 2
#define SAFETY_BRAKE_REMOTE 3
//OTHER
#define RPM_PER_KMH 55.0f // iMiev motor RPMs per kmh
#define PI 3.14159f
#define DEG180 180
//HANDBRAKE
#define HB_RANGE 400 //Encoder range (can be upto 1400)
#define HB_ADC_RANGE 4096
#define HB_KP 50 //PID proportional constant
#define HB_KI 0 //PID integral constant
#define HB_KD 1 //PID derivative constant
#define HB_MIN_DUTY_COEF 0.15f //
#define HB_MAX_DUTY_COEF 1 //
#define HB_CURRENT_LIMIT 500 //55mv = 1A, 0,055/(3,3/4096) = 62 per amp. 500 = 8.06A
#define MAX_HB_INTEGRAL 1024 //
#define HB_START_DELAY 1000 // in ms
//STRUCTS
struct my_car_parameters {
uint32_t gas_pedal_pid_output; //gas pedal pid dac output value
float speed_ms; //speed calculated from motor rpm
uint8_t gear; //current gear
float wheel_angle_rad; //wheel angle in radians
int wheel_angle_deg; //wheel angle in degrees
uint32_t steering_wheel_pwm; //
uint8_t safety_brake; //state of the safety brake
};
// Holds parameters requested from ROS
struct requested_car_parameters {
float velocity_ms; // m/s
float velocity_kmh; // km/h
float last_velocity_kmh; // kmh
float wheel_angle; // rad
float wheel_angle_deg; // degrees (0.5 precision)
float last_wheel_angle; // degrees (0.5 precision)
uint8_t safety_brake; // 0, 1, 2 requested safety brake state
};
// Current parameters saved from carre ECU CAN
struct car_ecu_parameters {
uint8_t gear; // int number (1-4)
uint8_t motor_temperatur; // Celsius
int32_t motor_rpm; // rounds per minute
uint8_t odometer; // km/h
uint8_t gas_pedal; // percentage
int32_t steering_wheel; // degrees (0.5 precision)
};
// PID controller, will be used for gas pedal and steering wheel angel calculations
struct pid_controller {
int kp; //proportional constant
int ki; //integral constant
int kd; //derivative cosntant
int error;
int integral;
float derivative;
int diff; //unnecessary, will be removed
uint32_t lastTick;
int previous_error;
int delay;
int output;
};
struct hb_controller {
int kp;
int ki;
int kd;
int error;
int integral;
float derivative;
int previous_error;
int delay;
int output;
uint32_t lastTick;
uint32_t min_duty;
uint32_t max_duty;
uint16_t position;
long current;
long setpoint;
};
struct counters {
uint8_t st_wheel;
uint8_t st_wheel_pid;
uint8_t gearbox;
uint8_t speed;
uint8_t gas_pedal;
uint8_t safety_brake;
uint8_t handbrake;
};
void executeGearChange(uint8_t new_gear);
uint8_t changeGear(uint8_t new_gear);
void changeSpeed(void);
void changeWheelAngle(void);
void sendGear(void);
void sendGasPedal(void);
void sendSteeringWheel(void);
void sendHandbrake(void);
void setSteeringWheelPWMOutput(void);
void setGasPedalDACOutput(void);
void setStartingParameters(void);
void setHandbrakePWMOutput(void);
void calculateGasControllerPID(int rpm);
void calculateHBControllerPID(void);
void calculateWheelControllerPID(void);
void startGasPedal(void);
void startHandbrake(void);
void startSteeringWheel(void);
/**
******************************************************************************
* File Name : gpio.h
* Description : This file contains all the functions prototypes for
* the gpio
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2018 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __gpio_H
#define __gpio_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include "main.h"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
void MX_GPIO_Init(void);
/* USER CODE BEGIN Prototypes */
/* USER CODE END Prototypes */
#ifdef __cplusplus
}
#endif
#endif /*__ pinoutConfig_H */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
/**
******************************************************************************
* File Name : main.hpp
* Description : This file contains the common defines of the application
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2018 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __MAIN_H
#define __MAIN_H
/* Includes ------------------------------------------------------------------*/
/* Includes ------------------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Private define ------------------------------------------------------------*/
#define HB_Current_Pin GPIO_PIN_0
#define HB_Current_GPIO_Port GPIOC
#define HB_LPWM_Pin GPIO_PIN_2
#define HB_LPWM_GPIO_Port GPIOA
#define HB_RPWM_Pin GPIO_PIN_3
#define HB_RPWM_GPIO_Port GPIOA
#define GAS_PEDAL_MAIN_Pin GPIO_PIN_4
#define GAS_PEDAL_MAIN_GPIO_Port GPIOA
#define GAS_PEDAL_SUB_Pin GPIO_PIN_5
#define GAS_PEDAL_SUB_GPIO_Port GPIOA
#define RGB_B_3_Pin GPIO_PIN_6
#define RGB_B_3_GPIO_Port GPIOA
#define RGB_G_3_Pin GPIO_PIN_7
#define RGB_G_3_GPIO_Port GPIOA
#define RGB_R_3_Pin GPIO_PIN_4
#define RGB_R_3_GPIO_Port GPIOC
#define RGB_B_1_Pin GPIO_PIN_5
#define RGB_B_1_GPIO_Port GPIOC
#define RGB_G_1_Pin GPIO_PIN_0
#define RGB_G_1_GPIO_Port GPIOB
#define RGB_R_1_Pin GPIO_PIN_1
#define RGB_R_1_GPIO_Port GPIOB
#define RGB_B_2_Pin GPIO_PIN_2
#define RGB_B_2_GPIO_Port GPIOB
#define RGB_G_2_Pin GPIO_PIN_7
#define RGB_G_2_GPIO_Port GPIOE
#define RGB_R_2_Pin GPIO_PIN_8
#define RGB_R_2_GPIO_Port GPIOE
#define GEAR_P_Pin GPIO_PIN_10
#define GEAR_P_GPIO_Port GPIOE
#define GEAR_R_Pin GPIO_PIN_11
#define GEAR_R_GPIO_Port GPIOE
#define GEAR_N_Pin GPIO_PIN_12
#define GEAR_N_GPIO_Port GPIOE
#define GEAR_D_Pin GPIO_PIN_13
#define GEAR_D_GPIO_Port GPIOE
#define GEAR_B_Pin GPIO_PIN_14
#define GEAR_B_GPIO_Port GPIOE
#define GEAR_C_Pin GPIO_PIN_15
#define GEAR_C_GPIO_Port GPIOE
#define HB_REn_Pin GPIO_PIN_10
#define HB_REn_GPIO_Port GPIOD
#define HB_LEn_Pin GPIO_PIN_11
#define HB_LEn_GPIO_Port GPIOD
#define HB_EncA_Pin GPIO_PIN_12
#define HB_EncA_GPIO_Port GPIOD
#define HB_EncB_Pin GPIO_PIN_13
#define HB_EncB_GPIO_Port GPIOD
#define DEBUG_TX_Pin GPIO_PIN_9
#define DEBUG_TX_GPIO_Port GPIOA
#define DEBUG_RX_Pin GPIO_PIN_10
#define DEBUG_RX_GPIO_Port GPIOA
#define ST_PWM_Pin GPIO_PIN_3
#define ST_PWM_GPIO_Port GPIOB
/* ########################## Assert Selection ############################## */
/**
* @brief Uncomment the line below to expanse the "assert_param" macro in the
* HAL drivers code
*/
/* #define USE_FULL_ASSERT 1U */
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
#ifdef __cplusplus
extern "C" {
#endif
void _Error_Handler(char *, int);
#define Error_Handler() _Error_Handler(__FILE__, __LINE__)
#ifdef __cplusplus
}
#endif
/**
* @}
*/
/**
* @}
*/
#endif /* __MAIN_H */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
//MCP4725 - DAC
#define MCP4725_MSG_SIZE 6 //in bytes
#define MCP4725_CODE 12U //Device code for MCP4725
#define MCP4725_DATA_WRITE 2 //Data send command code
#define MCP4725_DATA_WEOTE_EEPROM 3
#define MCP4725_PD_NORMAL 0U //Normal mode
#define MCP4725_PD_1K 1U //1k resistor to ground
#define MCP4725_PD_100K 2U //100k resistor to ground
#define MCP4725_PD_500K 3U //500k resistor to ground
#define MCP4725_SEND_TIMEOUT 10 //Transmit timeout in ms
#define MCP4725_ADDRESS 0xC0 //Device adress (1st byte from datasheet)
/**
******************************************************************************
* @file stm32f4xx_it.h
* @brief This file contains the headers of the interrupt handlers.
******************************************************************************
*
* COPYRIGHT(c) 2018 STMicroelectronics
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __STM32F4xx_IT_H
#define __STM32F4xx_IT_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include "main.h"
/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
/* Exported functions ------------------------------------------------------- */
void SysTick_Handler(void);
void EXTI0_IRQHandler(void);
void CAN1_TX_IRQHandler(void);
void CAN1_RX0_IRQHandler(void);
void CAN1_SCE_IRQHandler(void);
void TIM1_UP_TIM10_IRQHandler(void);
void USART1_IRQHandler(void);
void TIM6_DAC_IRQHandler(void);
void CAN2_TX_IRQHandler(void);
void CAN2_RX0_IRQHandler(void);
void CAN2_SCE_IRQHandler(void);
#ifdef __cplusplus
}
#endif
#endif /* __STM32F4xx_IT_H */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
/**
******************************************************************************
* File Name : TIM.h
* Description : This file provides code for the configuration
* of the TIM instances.
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2018 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __tim_H
#define __tim_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include "main.h"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
extern TIM_HandleTypeDef htim2;
extern TIM_HandleTypeDef htim4;
extern TIM_HandleTypeDef htim5;
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
extern void _Error_Handler(char *, int);
void MX_TIM2_Init(void);
void MX_TIM4_Init(void);
void MX_TIM5_Init(void);
void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
/* USER CODE BEGIN Prototypes */
/* USER CODE END Prototypes */
#ifdef __cplusplus
}
#endif
#endif /*__ tim_H */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
/**
******************************************************************************
* File Name : USART.h
* Description : This file provides code for the configuration
* of the USART instances.
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2018 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __usart_H
#define __usart_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include "main.h"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
extern UART_HandleTypeDef huart1;
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
extern void _Error_Handler(char *, int);
void MX_USART1_UART_Init(void);
/* USER CODE BEGIN Prototypes */
void PutChar(USART_TypeDef* USARTx, uint8_t ch);
/* USER CODE END Prototypes */
#ifdef __cplusplus
}
#endif
#endif /*__ usart_H */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
#include "stdint.h"
void char2float(char value[], float *p_result);
float convertMStoKMH(float speed_ms);
float convertRADtoFLOAT(float rad);
/*
FreeRTOS V9.0.0 - Copyright (C) 2016 Real Time Engineers Ltd.
All rights reserved
VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION.
This file is part of the FreeRTOS distribution.
FreeRTOS is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License (version 2) as published by the
Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception.
***************************************************************************
>>! NOTE: The modification to the GPL is included to allow you to !<<
>>! distribute a combined work that includes FreeRTOS without being !<<
>>! obliged to provide the source code for proprietary components !<<
>>! outside of the FreeRTOS kernel. !<<
***************************************************************************
FreeRTOS is distributed in the hope that it will be useful, but WITHOUT ANY
WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. Full license text is available on the following
link: http://www.freertos.org/a00114.html
***************************************************************************
* *
* FreeRTOS provides completely free yet professionally developed, *
* robust, strictly quality controlled, supported, and cross *
* platform software that is more than just the market leader, it *
* is the industry's de facto standard. *
* *
* Help yourself get started quickly while simultaneously helping *
* to support the FreeRTOS project by purchasing a FreeRTOS *
* tutorial book, reference manual, or both: *
* http://www.FreeRTOS.org/Documentation *
* *
***************************************************************************
http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading
the FAQ page "My application does not run, what could be wrong?". Have you
defined configASSERT()?
http://www.FreeRTOS.org/support - In return for receiving this top quality
embedded software for free we request you assist our global community by
participating in the support forum.
http://www.FreeRTOS.org/training - Investing in training allows your team to
be as productive as possible as early as possible. Now you can receive
FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers
Ltd, and the world's leading authority on the world's leading RTOS.
http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products,
including FreeRTOS+Trace - an indispensable productivity tool, a DOS
compatible FAT file system, and our tiny thread aware UDP/IP stack.
http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate.
Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS.
http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High
Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS
licenses offer ticketed support, indemnification and commercial middleware.
http://www.SafeRTOS.com - High Integrity Systems also provide a safety
engineered and independently SIL3 certified version for use in safety and
mission critical applications that require provable dependability.
1 tab == 4 spaces!
*/
#ifndef FREERTOS_CONFIG_H
#define FREERTOS_CONFIG_H
/*-----------------------------------------------------------
* Application specific definitions.
*
* These definitions should be adjusted for your particular hardware and
* application requirements.
*
* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE
* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE.
*
* See http://www.freertos.org/a00110.html.
*----------------------------------------------------------*/
/* Ensure stdint is only used by the compiler, and not the assembler. */
#if defined(__ICCARM__) || defined(__CC_ARM) || defined(__GNUC__)
#include <stdint.h>
extern uint32_t SystemCoreClock;
#endif
#define configUSE_PREEMPTION 1
#define configUSE_IDLE_HOOK 0
#define configUSE_TICK_HOOK 0
#define configCPU_CLOCK_HZ (SystemCoreClock)
#define configTICK_RATE_HZ ((TickType_t)1000)
#define configMAX_PRIORITIES (7)
#define configMINIMAL_STACK_SIZE ((uint16_t)128)
#define configTOTAL_HEAP_SIZE ((size_t)(15 * 1024))
#define configMAX_TASK_NAME_LEN (16)
#define configUSE_TRACE_FACILITY 1
#define configUSE_16_BIT_TICKS 0
#define configIDLE_SHOULD_YIELD 1
#define configUSE_MUTEXES 1
#define configQUEUE_REGISTRY_SIZE 8
#define configCHECK_FOR_STACK_OVERFLOW 0
#define configUSE_RECURSIVE_MUTEXES 1
#define configUSE_MALLOC_FAILED_HOOK 0
#define configUSE_APPLICATION_TASK_TAG 0
#define configUSE_COUNTING_SEMAPHORES 1
#define configGENERATE_RUN_TIME_STATS 0
/* Co-routine definitions. */
#define configUSE_CO_ROUTINES 0
#define configMAX_CO_ROUTINE_PRIORITIES (2)
/* Software timer definitions. */
#define configUSE_TIMERS 0
#define configTIMER_TASK_PRIORITY (2)
#define configTIMER_QUEUE_LENGTH 10
#define configTIMER_TASK_STACK_DEPTH (configMINIMAL_STACK_SIZE * 2)
/* Set the following definitions to 1 to include the API function, or zero
to exclude the API function. */
#define INCLUDE_vTaskPrioritySet 1
#define INCLUDE_uxTaskPriorityGet 1
#define INCLUDE_vTaskDelete 1
#define INCLUDE_vTaskCleanUpResources 0
#define INCLUDE_vTaskSuspend 1
#define INCLUDE_vTaskDelayUntil 0
#define INCLUDE_vTaskDelay 1
#define INCLUDE_xTaskGetSchedulerState 1
/* Cortex-M specific definitions. */
#ifdef __NVIC_PRIO_BITS
/* __BVIC_PRIO_BITS will be specified when CMSIS is being used. */
#define configPRIO_BITS __NVIC_PRIO_BITS
#else
#define configPRIO_BITS 4 /* 15 priority levels */
#endif
/* The lowest interrupt priority that can be used in a call to a "set priority"
function. */
#define configLIBRARY_LOWEST_INTERRUPT_PRIORITY 0xf
/* The highest interrupt priority that can be used by any interrupt service
routine that makes calls to interrupt safe FreeRTOS API functions. DO NOT CALL
INTERRUPT SAFE FREERTOS API FUNCTIONS FROM ANY INTERRUPT THAT HAS A HIGHER
PRIORITY THAN THIS! (higher priorities are lower numeric values. */
#define configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY 5
/* Interrupt priorities used by the kernel port layer itself. These are generic
to all Cortex-M ports, and do not rely on any particular library functions. */
#define configKERNEL_INTERRUPT_PRIORITY ( configLIBRARY_LOWEST_INTERRUPT_PRIORITY << (8 - configPRIO_BITS) )
/* !!!! configMAX_SYSCALL_INTERRUPT_PRIORITY must not be set to zero !!!!
See http://www.FreeRTOS.org/RTOS-Cortex-M3-M4.html. */
#define configMAX_SYSCALL_INTERRUPT_PRIORITY ( configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY << (8 - configPRIO_BITS) )
/* Normal assert() semantics without relying on the provision of an assert.h
header file. */
#define configASSERT( x ) if( ( x ) == 0 ) { taskDISABLE_INTERRUPTS(); for( ;; ); }
/* Definitions that map the FreeRTOS port interrupt handlers to their CMSIS
standard names. */
#define vPortSVCHandler SVC_Handler
#define xPortPendSVHandler PendSV_Handler
/* IMPORTANT: This define MUST be commented when used with STM32Cube firmware,
to prevent overwriting SysTick_Handler defined within STM32Cube HAL */
/* #define xPortSysTickHandler SysTick_Handler */
#endif /* FREERTOS_CONFIG_H */
/*
FreeRTOS V9.0.0 - Copyright (C) 2016 Real Time Engineers Ltd.
All rights reserved
VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION.
This file is part of the FreeRTOS distribution.
FreeRTOS is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License (version 2) as published by the
Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception.
***************************************************************************
>>! NOTE: The modification to the GPL is included to allow you to !<<
>>! distribute a combined work that includes FreeRTOS without being !<<
>>! obliged to provide the source code for proprietary components !<<
>>! outside of the FreeRTOS kernel. !<<
***************************************************************************
FreeRTOS is distributed in the hope that it will be useful, but WITHOUT ANY
WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. Full license text is available on the following
link: http://www.freertos.org/a00114.html
***************************************************************************
* *
* FreeRTOS provides completely free yet professionally developed, *
* robust, strictly quality controlled, supported, and cross *
* platform software that is more than just the market leader, it *
* is the industry's de facto standard. *
* *
* Help yourself get started quickly while simultaneously helping *
* to support the FreeRTOS project by purchasing a FreeRTOS *
* tutorial book, reference manual, or both: *
* http://www.FreeRTOS.org/Documentation *
* *
***************************************************************************
http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading
the FAQ page "My application does not run, what could be wrong?". Have you
defined configASSERT()?
http://www.FreeRTOS.org/support - In return for receiving this top quality
embedded software for free we request you assist our global community by
participating in the support forum.
http://www.FreeRTOS.org/training - Investing in training allows your team to
be as productive as possible as early as possible. Now you can receive
FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers
Ltd, and the world's leading authority on the world's leading RTOS.
http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products,
including FreeRTOS+Trace - an indispensable productivity tool, a DOS
compatible FAT file system, and our tiny thread aware UDP/IP stack.
http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate.
Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS.
http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High
Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS
licenses offer ticketed support, indemnification and commercial middleware.
http://www.SafeRTOS.com - High Integrity Systems also provide a safety
engineered and independently SIL3 certified version for use in safety and
mission critical applications that require provable dependability.
1 tab == 4 spaces!
*/
#ifndef STACK_MACROS_H
#define STACK_MACROS_H
/*
* Call the stack overflow hook function if the stack of the task being swapped
* out is currently overflowed, or looks like it might have overflowed in the
* past.
*
* Setting configCHECK_FOR_STACK_OVERFLOW to 1 will cause the macro to check
* the current stack state only - comparing the current top of stack value to
* the stack limit. Setting configCHECK_FOR_STACK_OVERFLOW to greater than 1
* will also cause the last few stack bytes to be checked to ensure the value
* to which the bytes were set when the task was created have not been
* overwritten. Note this second test does not guarantee that an overflowed
* stack will always be recognised.
*/
/*-----------------------------------------------------------*/
#if( ( configCHECK_FOR_STACK_OVERFLOW == 1 ) && ( portSTACK_GROWTH < 0 ) )
/* Only the current stack state is to be checked. */
#define taskCHECK_FOR_STACK_OVERFLOW() \
{ \
/* Is the currently saved stack pointer within the stack limit? */ \
if( pxCurrentTCB->pxTopOfStack <= pxCurrentTCB->pxStack ) \
{ \
vApplicationStackOverflowHook( ( TaskHandle_t ) pxCurrentTCB, pxCurrentTCB->pcTaskName ); \
} \
}
#endif /* configCHECK_FOR_STACK_OVERFLOW == 1 */
/*-----------------------------------------------------------*/
#if( ( configCHECK_FOR_STACK_OVERFLOW == 1 ) && ( portSTACK_GROWTH > 0 ) )
/* Only the current stack state is to be checked. */
#define taskCHECK_FOR_STACK_OVERFLOW() \
{ \
\
/* Is the currently saved stack pointer within the stack limit? */ \
if( pxCurrentTCB->pxTopOfStack >= pxCurrentTCB->pxEndOfStack ) \
{ \
vApplicationStackOverflowHook( ( TaskHandle_t ) pxCurrentTCB, pxCurrentTCB->pcTaskName ); \
} \
}
#endif /* configCHECK_FOR_STACK_OVERFLOW == 1 */
/*-----------------------------------------------------------*/
#if( ( configCHECK_FOR_STACK_OVERFLOW > 1 ) && ( portSTACK_GROWTH < 0 ) )
#define taskCHECK_FOR_STACK_OVERFLOW() \
{ \
const uint32_t * const pulStack = ( uint32_t * ) pxCurrentTCB->pxStack; \
const uint32_t ulCheckValue = ( uint32_t ) 0xa5a5a5a5; \
\
if( ( pulStack[ 0 ] != ulCheckValue ) || \
( pulStack[ 1 ] != ulCheckValue ) || \
( pulStack[ 2 ] != ulCheckValue ) || \
( pulStack[ 3 ] != ulCheckValue ) ) \
{ \
vApplicationStackOverflowHook( ( TaskHandle_t ) pxCurrentTCB, pxCurrentTCB->pcTaskName ); \
} \
}
#endif /* #if( configCHECK_FOR_STACK_OVERFLOW > 1 ) */
/*-----------------------------------------------------------*/
#if( ( configCHECK_FOR_STACK_OVERFLOW > 1 ) && ( portSTACK_GROWTH > 0 ) )
#define taskCHECK_FOR_STACK_OVERFLOW() \
{ \
int8_t *pcEndOfStack = ( int8_t * ) pxCurrentTCB->pxEndOfStack; \
static const uint8_t ucExpectedStackBytes[] = { tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, \
tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, \
tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, \
tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, \
tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE, tskSTACK_FILL_BYTE }; \
\
\
pcEndOfStack -= sizeof( ucExpectedStackBytes ); \
\
/* Has the extremity of the task stack ever been written over? */ \
if( memcmp( ( void * ) pcEndOfStack, ( void * ) ucExpectedStackBytes, sizeof( ucExpectedStackBytes ) ) != 0 ) \
{ \
vApplicationStackOverflowHook( ( TaskHandle_t ) pxCurrentTCB, pxCurrentTCB->pcTaskName ); \
} \
}
#endif /* #if( configCHECK_FOR_STACK_OVERFLOW > 1 ) */
/*-----------------------------------------------------------*/
/* Remove stack overflow macro if not being used. */
#ifndef taskCHECK_FOR_STACK_OVERFLOW
#define taskCHECK_FOR_STACK_OVERFLOW()
#endif
#endif /* STACK_MACROS_H */
/*
FreeRTOS V9.0.0 - Copyright (C) 2016 Real Time Engineers Ltd.
All rights reserved
VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION.
This file is part of the FreeRTOS distribution.
FreeRTOS is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License (version 2) as published by the
Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception.
***************************************************************************
>>! NOTE: The modification to the GPL is included to allow you to !<<
>>! distribute a combined work that includes FreeRTOS without being !<<
>>! obliged to provide the source code for proprietary components !<<
>>! outside of the FreeRTOS kernel. !<<
***************************************************************************
FreeRTOS is distributed in the hope that it will be useful, but WITHOUT ANY
WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. Full license text is available on the following
link: http://www.freertos.org/a00114.html
***************************************************************************
* *
* FreeRTOS provides completely free yet professionally developed, *
* robust, strictly quality controlled, supported, and cross *
* platform software that is more than just the market leader, it *
* is the industry's de facto standard. *
* *
* Help yourself get started quickly while simultaneously helping *
* to support the FreeRTOS project by purchasing a FreeRTOS *
* tutorial book, reference manual, or both: *
* http://www.FreeRTOS.org/Documentation *
* *
***************************************************************************
http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading
the FAQ page "My application does not run, what could be wrong?". Have you
defined configASSERT()?
http://www.FreeRTOS.org/support - In return for receiving this top quality
embedded software for free we request you assist our global community by
participating in the support forum.
http://www.FreeRTOS.org/training - Investing in training allows your team to
be as productive as possible as early as possible. Now you can receive
FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers
Ltd, and the world's leading authority on the world's leading RTOS.
http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products,
including FreeRTOS+Trace - an indispensable productivity tool, a DOS
compatible FAT file system, and our tiny thread aware UDP/IP stack.
http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate.
Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS.
http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High
Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS
licenses offer ticketed support, indemnification and commercial middleware.
http://www.SafeRTOS.com - High Integrity Systems also provide a safety
engineered and independently SIL3 certified version for use in safety and
mission critical applications that require provable dependability.
1 tab == 4 spaces!
*/
/*-----------------------------------------------------------
* Portable layer API. Each function must be defined for each port.
*----------------------------------------------------------*/
#ifndef PORTABLE_H
#define PORTABLE_H
/* Each FreeRTOS port has a unique portmacro.h header file. Originally a
pre-processor definition was used to ensure the pre-processor found the correct
portmacro.h file for the port being used. That scheme was deprecated in favour
of setting the compiler's include path such that it found the correct
portmacro.h file - removing the need for the constant and allowing the
portmacro.h file to be located anywhere in relation to the port being used.
Purely for reasons of backward compatibility the old method is still valid, but
to make it clear that new projects should not use it, support for the port
specific constants has been moved into the deprecated_definitions.h header
file. */
#include "deprecated_definitions.h"
/* If portENTER_CRITICAL is not defined then including deprecated_definitions.h
did not result in a portmacro.h header file being included - and it should be
included here. In this case the path to the correct portmacro.h header file
must be set in the compiler's include path. */
#ifndef portENTER_CRITICAL
#include "portmacro.h"
#endif
#if portBYTE_ALIGNMENT == 32
#define portBYTE_ALIGNMENT_MASK ( 0x001f )
#endif
#if portBYTE_ALIGNMENT == 16
#define portBYTE_ALIGNMENT_MASK ( 0x000f )
#endif
#if portBYTE_ALIGNMENT == 8
#define portBYTE_ALIGNMENT_MASK ( 0x0007 )
#endif
#if portBYTE_ALIGNMENT == 4
#define portBYTE_ALIGNMENT_MASK ( 0x0003 )
#endif
#if portBYTE_ALIGNMENT == 2
#define portBYTE_ALIGNMENT_MASK ( 0x0001 )
#endif
#if portBYTE_ALIGNMENT == 1
#define portBYTE_ALIGNMENT_MASK ( 0x0000 )
#endif
#ifndef portBYTE_ALIGNMENT_MASK
#error "Invalid portBYTE_ALIGNMENT definition"
#endif
#ifndef portNUM_CONFIGURABLE_REGIONS
#define portNUM_CONFIGURABLE_REGIONS 1
#endif
#ifdef __cplusplus
extern "C" {
#endif
#include "mpu_wrappers.h"
/*
* Setup the stack of a new task so it is ready to be placed under the
* scheduler control. The registers have to be placed on the stack in
* the order that the port expects to find them.
*
*/
#if( portUSING_MPU_WRAPPERS == 1 )
PRIVILEGED_FUNCTION StackType_t *pxPortInitialiseStack( StackType_t *pxTopOfStack, TaskFunction_t pxCode, void *pvParameters, BaseType_t xRunPrivileged ) ;
#else
PRIVILEGED_FUNCTION StackType_t *pxPortInitialiseStack( StackType_t *pxTopOfStack, TaskFunction_t pxCode, void *pvParameters ) ;
#endif
/* Used by heap_5.c. */
typedef struct HeapRegion
{
uint8_t *pucStartAddress;
size_t xSizeInBytes;
} HeapRegion_t;
/*
* Used to define multiple heap regions for use by heap_5.c. This function
* must be called before any calls to pvPortMalloc() - not creating a task,
* queue, semaphore, mutex, software timer, event group, etc. will result in
* pvPortMalloc being called.
*
* pxHeapRegions passes in an array of HeapRegion_t structures - each of which
* defines a region of memory that can be used as the heap. The array is
* terminated by a HeapRegions_t structure that has a size of 0. The region
* with the lowest start address must appear first in the array.
*/
PRIVILEGED_FUNCTION void vPortDefineHeapRegions( const HeapRegion_t * const pxHeapRegions );
/*
* Map to the memory management routines required for the port.
*/
PRIVILEGED_FUNCTION void *pvPortMalloc( size_t xSize );
PRIVILEGED_FUNCTION void vPortFree( void *pv );
PRIVILEGED_FUNCTION void vPortInitialiseBlocks( void );
PRIVILEGED_FUNCTION size_t xPortGetFreeHeapSize( void );
PRIVILEGED_FUNCTION size_t xPortGetMinimumEverFreeHeapSize( void );
/*
* Setup the hardware ready for the scheduler to take control. This generally
* sets up a tick interrupt and sets timers for the correct tick frequency.
*/
PRIVILEGED_FUNCTION BaseType_t xPortStartScheduler( void );
/*
* Undo any hardware/ISR setup that was performed by xPortStartScheduler() so
* the hardware is left in its original condition after the scheduler stops
* executing.
*/
PRIVILEGED_FUNCTION void vPortEndScheduler( void );
/*
* The structures and methods of manipulating the MPU are contained within the
* port layer.
*
* Fills the xMPUSettings structure with the memory region information
* contained in xRegions.
*/
#if( portUSING_MPU_WRAPPERS == 1 )
struct xMEMORY_REGION;
PRIVILEGED_FUNCTION void vPortStoreTaskMPUSettings( xMPU_SETTINGS *xMPUSettings, const struct xMEMORY_REGION * const xRegions, StackType_t *pxBottomOfStack, uint32_t ulStackDepth );
#endif
#ifdef __cplusplus
}
#endif
#endif /* PORTABLE_H */
/*
FreeRTOS V9.0.0 - Copyright (C) 2016 Real Time Engineers Ltd.
All rights reserved
VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION.
This file is part of the FreeRTOS distribution.
FreeRTOS is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License (version 2) as published by the
Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception.
***************************************************************************
>>! NOTE: The modification to the GPL is included to allow you to !<<
>>! distribute a combined work that includes FreeRTOS without being !<<
>>! obliged to provide the source code for proprietary components !<<
>>! outside of the FreeRTOS kernel. !<<
***************************************************************************
FreeRTOS is distributed in the hope that it will be useful, but WITHOUT ANY
WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. Full license text is available on the following
link: http://www.freertos.org/a00114.html
***************************************************************************
* *
* FreeRTOS provides completely free yet professionally developed, *
* robust, strictly quality controlled, supported, and cross *
* platform software that is more than just the market leader, it *
* is the industry's de facto standard. *
* *
* Help yourself get started quickly while simultaneously helping *
* to support the FreeRTOS project by purchasing a FreeRTOS *
* tutorial book, reference manual, or both: *
* http://www.FreeRTOS.org/Documentation *
* *
***************************************************************************
http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading
the FAQ page "My application does not run, what could be wrong?". Have you
defined configASSERT()?
http://www.FreeRTOS.org/support - In return for receiving this top quality
embedded software for free we request you assist our global community by
participating in the support forum.
http://www.FreeRTOS.org/training - Investing in training allows your team to
be as productive as possible as early as possible. Now you can receive
FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers
Ltd, and the world's leading authority on the world's leading RTOS.
http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products,
including FreeRTOS+Trace - an indispensable productivity tool, a DOS
compatible FAT file system, and our tiny thread aware UDP/IP stack.
http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate.
Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS.
http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High
Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS
licenses offer ticketed support, indemnification and commercial middleware.
http://www.SafeRTOS.com - High Integrity Systems also provide a safety
engineered and independently SIL3 certified version for use in safety and
mission critical applications that require provable dependability.
1 tab == 4 spaces!
*/
#ifndef PROJDEFS_H
#define PROJDEFS_H
/*
* Defines the prototype to which task functions must conform. Defined in this
* file to ensure the type is known before portable.h is included.
*/
typedef void (*TaskFunction_t)( void * );
/* Converts a time in milliseconds to a time in ticks. This macro can be
overridden by a macro of the same name defined in FreeRTOSConfig.h in case the
definition here is not suitable for your application. */
#ifndef pdMS_TO_TICKS
#define pdMS_TO_TICKS( xTimeInMs ) ( ( TickType_t ) ( ( ( TickType_t ) ( xTimeInMs ) * ( TickType_t ) configTICK_RATE_HZ ) / ( TickType_t ) 1000 ) )
#endif
#define pdFALSE ( ( BaseType_t ) 0 )
#define pdTRUE ( ( BaseType_t ) 1 )
#define pdPASS ( pdTRUE )
#define pdFAIL ( pdFALSE )
#define errQUEUE_EMPTY ( ( BaseType_t ) 0 )
#define errQUEUE_FULL ( ( BaseType_t ) 0 )
/* FreeRTOS error definitions. */
#define errCOULD_NOT_ALLOCATE_REQUIRED_MEMORY ( -1 )
#define errQUEUE_BLOCKED ( -4 )
#define errQUEUE_YIELD ( -5 )
/* Macros used for basic data corruption checks. */
#ifndef configUSE_LIST_DATA_INTEGRITY_CHECK_BYTES
#define configUSE_LIST_DATA_INTEGRITY_CHECK_BYTES 0
#endif
#if( configUSE_16_BIT_TICKS == 1 )
#define pdINTEGRITY_CHECK_VALUE 0x5a5a
#else
#define pdINTEGRITY_CHECK_VALUE 0x5a5a5a5aUL
#endif
/* The following errno values are used by FreeRTOS+ components, not FreeRTOS
itself. */
#define pdFREERTOS_ERRNO_NONE 0 /* No errors */
#define pdFREERTOS_ERRNO_ENOENT 2 /* No such file or directory */
#define pdFREERTOS_ERRNO_EINTR 4 /* Interrupted system call */
#define pdFREERTOS_ERRNO_EIO 5 /* I/O error */
#define pdFREERTOS_ERRNO_ENXIO 6 /* No such device or address */
#define pdFREERTOS_ERRNO_EBADF 9 /* Bad file number */
#define pdFREERTOS_ERRNO_EAGAIN 11 /* No more processes */
#define pdFREERTOS_ERRNO_EWOULDBLOCK 11 /* Operation would block */
#define pdFREERTOS_ERRNO_ENOMEM 12 /* Not enough memory */
#define pdFREERTOS_ERRNO_EACCES 13 /* Permission denied */
#define pdFREERTOS_ERRNO_EFAULT 14 /* Bad address */
#define pdFREERTOS_ERRNO_EBUSY 16 /* Mount device busy */
#define pdFREERTOS_ERRNO_EEXIST 17 /* File exists */
#define pdFREERTOS_ERRNO_EXDEV 18 /* Cross-device link */
#define pdFREERTOS_ERRNO_ENODEV 19 /* No such device */
#define pdFREERTOS_ERRNO_ENOTDIR 20 /* Not a directory */
#define pdFREERTOS_ERRNO_EISDIR 21 /* Is a directory */
#define pdFREERTOS_ERRNO_EINVAL 22 /* Invalid argument */
#define pdFREERTOS_ERRNO_ENOSPC 28 /* No space left on device */
#define pdFREERTOS_ERRNO_ESPIPE 29 /* Illegal seek */
#define pdFREERTOS_ERRNO_EROFS 30 /* Read only file system */
#define pdFREERTOS_ERRNO_EUNATCH 42 /* Protocol driver not attached */
#define pdFREERTOS_ERRNO_EBADE 50 /* Invalid exchange */
#define pdFREERTOS_ERRNO_EFTYPE 79 /* Inappropriate file type or format */
#define pdFREERTOS_ERRNO_ENMFILE 89 /* No more files */
#define pdFREERTOS_ERRNO_ENOTEMPTY 90 /* Directory not empty */
#define pdFREERTOS_ERRNO_ENAMETOOLONG 91 /* File or path name too long */
#define pdFREERTOS_ERRNO_EOPNOTSUPP 95 /* Operation not supported on transport endpoint */
#define pdFREERTOS_ERRNO_ENOBUFS 105 /* No buffer space available */
#define pdFREERTOS_ERRNO_ENOPROTOOPT 109 /* Protocol not available */
#define pdFREERTOS_ERRNO_EADDRINUSE 112 /* Address already in use */
#define pdFREERTOS_ERRNO_ETIMEDOUT 116 /* Connection timed out */
#define pdFREERTOS_ERRNO_EINPROGRESS 119 /* Connection already in progress */
#define pdFREERTOS_ERRNO_EALREADY 120 /* Socket already connected */
#define pdFREERTOS_ERRNO_EADDRNOTAVAIL 125 /* Address not available */
#define pdFREERTOS_ERRNO_EISCONN 127 /* Socket is already connected */
#define pdFREERTOS_ERRNO_ENOTCONN 128 /* Socket is not connected */
#define pdFREERTOS_ERRNO_ENOMEDIUM 135 /* No medium inserted */
#define pdFREERTOS_ERRNO_EILSEQ 138 /* An invalid UTF-16 sequence was encountered. */
#define pdFREERTOS_ERRNO_ECANCELED 140 /* Operation canceled. */
/* The following endian values are used by FreeRTOS+ components, not FreeRTOS
itself. */
#define pdFREERTOS_LITTLE_ENDIAN 0
#define pdFREERTOS_BIG_ENDIAN 1
#endif /* PROJDEFS_H */
This source diff could not be displayed because it is too large. You can view the blob instead.
/**
******************************************************************************
* File Name : ADC.c
* Description : This file provides code for the configuration
* of the ADC instances.
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2018 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "adc.h"
#include "gpio.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
ADC_HandleTypeDef hadc1;
/* ADC1 init function */
void MX_ADC1_Init(void)
{
ADC_ChannelConfTypeDef sConfig;
/**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
*/
hadc1.Instance = ADC1;
hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
hadc1.Init.Resolution = ADC_RESOLUTION_12B;
hadc1.Init.ScanConvMode = DISABLE;
hadc1.Init.ContinuousConvMode = ENABLE;
hadc1.Init.DiscontinuousConvMode = DISABLE;
hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc1.Init.NbrOfConversion = 1;
hadc1.Init.DMAContinuousRequests = DISABLE;
hadc1.Init.EOCSelection = ADC_EOC_SEQ_CONV;
if (HAL_ADC_Init(&hadc1) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
/**Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
*/
sConfig.Channel = ADC_CHANNEL_10;
sConfig.Rank = 1;
sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
}
void HAL_ADC_MspInit(ADC_HandleTypeDef* adcHandle)
{
GPIO_InitTypeDef GPIO_InitStruct;
if(adcHandle->Instance==ADC1)
{
/* USER CODE BEGIN ADC1_MspInit 0 */
/* USER CODE END ADC1_MspInit 0 */
/* ADC1 clock enable */
__HAL_RCC_ADC1_CLK_ENABLE();
/**ADC1 GPIO Configuration
PC0 ------> ADC1_IN10
*/
GPIO_InitStruct.Pin = HB_Current_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(HB_Current_GPIO_Port, &GPIO_InitStruct);
/* USER CODE BEGIN ADC1_MspInit 1 */
/* USER CODE END ADC1_MspInit 1 */
}
}
void HAL_ADC_MspDeInit(ADC_HandleTypeDef* adcHandle)
{
if(adcHandle->Instance==ADC1)
{
/* USER CODE BEGIN ADC1_MspDeInit 0 */
/* USER CODE END ADC1_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_ADC1_CLK_DISABLE();
/**ADC1 GPIO Configuration
PC0 ------> ADC1_IN10
*/
HAL_GPIO_DeInit(HB_Current_GPIO_Port, HB_Current_Pin);
/* USER CODE BEGIN ADC1_MspDeInit 1 */
/* USER CODE END ADC1_MspDeInit 1 */
}
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
/**
******************************************************************************
* File Name : DAC.c
* Description : This file provides code for the configuration
* of the DAC instances.
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2018 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "dac.h"
#include "gpio.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
DAC_HandleTypeDef hdac;
/* DAC init function */
void MX_DAC_Init(void)
{
DAC_ChannelConfTypeDef sConfig;
/**DAC Initialization
*/
hdac.Instance = DAC;
if (HAL_DAC_Init(&hdac) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
/**DAC channel OUT1 config
*/
sConfig.DAC_Trigger = DAC_TRIGGER_NONE;
sConfig.DAC_OutputBuffer = DAC_OUTPUTBUFFER_ENABLE;
if (HAL_DAC_ConfigChannel(&hdac, &sConfig, DAC_CHANNEL_1) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
/**DAC channel OUT2 config
*/
if (HAL_DAC_ConfigChannel(&hdac, &sConfig, DAC_CHANNEL_2) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
}
void HAL_DAC_MspInit(DAC_HandleTypeDef* dacHandle)
{
GPIO_InitTypeDef GPIO_InitStruct;
if(dacHandle->Instance==DAC)
{
/* USER CODE BEGIN DAC_MspInit 0 */
/* USER CODE END DAC_MspInit 0 */
/* DAC clock enable */
__HAL_RCC_DAC_CLK_ENABLE();
/**DAC GPIO Configuration
PA4 ------> DAC_OUT1
PA5 ------> DAC_OUT2
*/
GPIO_InitStruct.Pin = GAS_PEDAL_MAIN_Pin|GAS_PEDAL_SUB_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/* DAC interrupt Init */
HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn);
/* USER CODE BEGIN DAC_MspInit 1 */
/* USER CODE END DAC_MspInit 1 */
}
}
void HAL_DAC_MspDeInit(DAC_HandleTypeDef* dacHandle)
{
if(dacHandle->Instance==DAC)
{
/* USER CODE BEGIN DAC_MspDeInit 0 */
/* USER CODE END DAC_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_DAC_CLK_DISABLE();
/**DAC GPIO Configuration
PA4 ------> DAC_OUT1
PA5 ------> DAC_OUT2
*/
HAL_GPIO_DeInit(GPIOA, GAS_PEDAL_MAIN_Pin|GAS_PEDAL_SUB_Pin);
/* DAC interrupt Deinit */
HAL_NVIC_DisableIRQ(TIM6_DAC_IRQn);
/* USER CODE BEGIN DAC_MspDeInit 1 */
/* USER CODE END DAC_MspDeInit 1 */
}
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
/**
******************************************************************************
* File Name : freertos.c
* Description : Code for freertos applications
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2018 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "cmsis_os.h"
/* USER CODE BEGIN Includes */
#include "can.h"
#include "drive.h"
#include "utils.h"
#include "stdlib.h"
#include "usart.h"
#include "string.h"
/* USER CODE END Includes */
/* Variables -----------------------------------------------------------------*/
osThreadId MainTaskHandle;
osThreadId GasTaskHandle;
/* USER CODE BEGIN Variables */
extern struct car_ecu_parameters g_car_ecu;
extern struct requested_car_parameters g_requested_data;
extern struct pid_controller g_wheel_controller;
extern UART_HandleTypeDef huart1;
/* USER CODE END Variables */
/* Function prototypes -------------------------------------------------------*/
void MainThread(void const * argument);
void GasThread(void const * argument);
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
/* USER CODE BEGIN FunctionPrototypes */
/* USER CODE END FunctionPrototypes */
/* Hook prototypes */
/* Init FreeRTOS */
void MX_FREERTOS_Init(void) {
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* USER CODE BEGIN RTOS_MUTEX */
/* add mutexes, ... */
/* USER CODE END RTOS_MUTEX */
/* USER CODE BEGIN RTOS_SEMAPHORES */
/* add semaphores, ... */
/* USER CODE END RTOS_SEMAPHORES */
/* USER CODE BEGIN RTOS_TIMERS */
/* start timers, add new ones, ... */
/* USER CODE END RTOS_TIMERS */
/* Create the thread(s) */
/* definition and creation of MainTask */
osThreadDef(MainTask, MainThread, osPriorityHigh, 0, 128);
MainTaskHandle = osThreadCreate(osThread(MainTask), NULL);
/* definition and creation of GasTask */
osThreadDef(GasTask, GasThread, osPriorityHigh, 0, 128);
GasTaskHandle = osThreadCreate(osThread(GasTask), NULL);
/* USER CODE BEGIN RTOS_THREADS */
/* add threads, ... */
/* USER CODE END RTOS_THREADS */
/* USER CODE BEGIN RTOS_QUEUES */
/* add queues, ... */
/* USER CODE END RTOS_QUEUES */
}
/* MainThread function */
void MainThread(void const * argument)
{
/* USER CODE BEGIN MainThread */
char txData[30] = "Main thread started\r\n";
HAL_UART_Transmit(&huart1, (uint8_t *)txData, strlen(txData),10);
setStartingParameters();
HAL_GPIO_WritePin(RGB_G_3_GPIO_Port, RGB_G_3_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(RGB_R_3_GPIO_Port, RGB_R_3_Pin, GPIO_PIN_RESET);
/* Infinite loop */
for(;;)
{
strcpy(txData,"Terminating main thread\r\n");
HAL_UART_Transmit(&huart1, (uint8_t *)txData, strlen(txData),10);
osThreadTerminate(NULL);
}
/* USER CODE END MainThread */
}
/* GasThread function */
void GasThread(void const * argument)
{
/* USER CODE BEGIN GasThread */
char txData[30] = "Gas thread started\r\n";
HAL_UART_Transmit(&huart1, (uint8_t *)txData, strlen(txData),10);
int c = 0; // counter used for debbuging purpose
int p = 0;
startGasPedal();
setGasPedalDACOutput();
if (changeGear(GEAR_P) != GEAR_CHANGE_READY) {
//TODO
}
startSteeringWheel();
setSteeringWheelPWMOutput();
// startHandbrake();
HAL_GPIO_WritePin(RGB_R_3_GPIO_Port, RGB_R_3_Pin, GPIO_PIN_SET);
/* Infinite loop */
for(;;) {
c++;
if (HAL_GetTick() - p > 100) {
HAL_GPIO_TogglePin(RGB_B_2_GPIO_Port, RGB_B_2_Pin);
p = HAL_GetTick();
}
hcan1.pTxMsg->StdId = 0x321;
hcan1.pTxMsg->IDE = CAN_ID_STD;
hcan1.pTxMsg->RTR = CAN_RTR_DATA;
hcan1.pTxMsg->DLC = 4;
hcan1.pTxMsg->Data[0] = abs(convertMStoKMH(g_requested_data.velocity_ms));
hcan1.pTxMsg->Data[1] = g_car_ecu.odometer;
hcan1.pTxMsg->Data[2] = c/256;
hcan1.pTxMsg->Data[3] = c%256;
// HAL_CAN_Transmit_IT(&hcan1);
/* if(convertMStoKMH(g_requested_data.velocity) == 0){
if (changeGear(GEAR_P) != GEAR_CHANGE_READY) {
//TODO
}
}
if (g_car_ecu.odometer != abs(convertMStoKMH(g_requested_data.velocity))) {
if (g_requested_data.velocity > 0) {
if (changeGear(GEAR_D) != GEAR_CHANGE_READY) {
//TODO
}
} else if (g_requested_data.velocity < 0) {
if (changeGear(GEAR_R) != GEAR_CHANGE_READY) {
//TODO
}
} else if (g_requested_data.velocity == 0) {
//TODO
}
//changeSpeed();
}*/
changeSpeed();
changeWheelAngle();
// osDelay(1);
}
/* USER CODE END GasThread */
}
/* USER CODE BEGIN Application */
/* USER CODE END Application */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
/**
******************************************************************************
* File Name : gpio.c
* Description : This file provides code for the configuration
* of all used GPIO pins.
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2018 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "gpio.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/*----------------------------------------------------------------------------*/
/* Configure GPIO */
/*----------------------------------------------------------------------------*/
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/** Configure pins as
* Analog
* Input
* Output
* EVENT_OUT
* EXTI
*/
void MX_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
/* GPIO Ports Clock Enable */
__HAL_RCC_GPIOH_CLK_ENABLE();
__HAL_RCC_GPIOC_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_GPIOE_CLK_ENABLE();
__HAL_RCC_GPIOD_CLK_ENABLE();
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOA, RGB_B_3_Pin|RGB_G_3_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOC, RGB_R_3_Pin|RGB_B_1_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOB, RGB_G_1_Pin|RGB_R_1_Pin|RGB_B_2_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOE, RGB_G_2_Pin|RGB_R_2_Pin|GEAR_P_Pin|GEAR_R_Pin
|GEAR_N_Pin|GEAR_D_Pin|GEAR_B_Pin|GEAR_C_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOD, HB_REn_Pin|HB_LEn_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin : PA0 */
GPIO_InitStruct.Pin = GPIO_PIN_0;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/*Configure GPIO pins : PAPin PAPin */
GPIO_InitStruct.Pin = RGB_B_3_Pin|RGB_G_3_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/*Configure GPIO pins : PCPin PCPin */
GPIO_InitStruct.Pin = RGB_R_3_Pin|RGB_B_1_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/*Configure GPIO pins : PBPin PBPin PBPin */
GPIO_InitStruct.Pin = RGB_G_1_Pin|RGB_R_1_Pin|RGB_B_2_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/*Configure GPIO pins : PEPin PEPin PEPin PEPin
PEPin PEPin PEPin PEPin */
GPIO_InitStruct.Pin = RGB_G_2_Pin|RGB_R_2_Pin|GEAR_P_Pin|GEAR_R_Pin
|GEAR_N_Pin|GEAR_D_Pin|GEAR_B_Pin|GEAR_C_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
/*Configure GPIO pins : PDPin PDPin */
GPIO_InitStruct.Pin = HB_REn_Pin|HB_LEn_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
/* EXTI interrupt init*/
HAL_NVIC_SetPriority(EXTI0_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(EXTI0_IRQn);
}
/* USER CODE BEGIN 2 */
/* USER CODE END 2 */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
/**
******************************************************************************
* File Name : main.c
* Description : Main program body
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2018 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "stm32f4xx_hal.h"
#include "cmsis_os.h"
#include "adc.h"
#include "can.h"
#include "dac.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
/* USER CODE BEGIN Includes */
#include "drive.h"
/* USER CODE END Includes */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
struct requested_car_parameters g_requested_data;
struct car_ecu_parameters g_car_ecu;
struct pid_controller g_gas_controller;
struct pid_controller g_wheel_controller;
struct my_car_parameters g_my_car;
struct hb_controller g_hb_controller;
struct counters g_counters;
/* Private variables ---------------------------------------------------------*/
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
void MX_FREERTOS_Init(void);
/* USER CODE BEGIN PFP */
/* Private function prototypes -----------------------------------------------*/
/* USER CODE END PFP */
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration----------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_DAC_Init();
MX_ADC1_Init();
MX_CAN1_Init();
MX_CAN2_Init();
MX_TIM5_Init();
MX_TIM4_Init();
MX_TIM2_Init();
MX_USART1_UART_Init();
/* USER CODE BEGIN 2 */
//HAL_GPIO_WritePin(GPIOD, LED_RED_Pin, GPIO_PIN_SET);
//convertMStoKMH(g_requested_data.velocity);
HAL_GPIO_WritePin(RGB_G_3_GPIO_Port, RGB_G_3_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(RGB_G_2_GPIO_Port, RGB_G_2_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(RGB_G_1_GPIO_Port, RGB_G_1_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(RGB_B_3_GPIO_Port, RGB_B_3_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(RGB_B_2_GPIO_Port, RGB_B_2_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(RGB_B_1_GPIO_Port, RGB_B_1_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(RGB_R_3_GPIO_Port, RGB_R_3_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(RGB_R_2_GPIO_Port, RGB_R_2_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(RGB_R_1_GPIO_Port, RGB_R_1_Pin, GPIO_PIN_SET);
/* USER CODE END 2 */
/* Call init function for freertos objects (in freertos.c) */
MX_FREERTOS_Init();
/* Start scheduler */
osKernelStart();
/* We should never get here as control is now taken by the scheduler */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
/** System Clock Configuration
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct;
RCC_ClkInitTypeDef RCC_ClkInitStruct;
/**Configure the main internal regulator output voltage
*/
__HAL_RCC_PWR_CLK_ENABLE();
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
/**Initializes the CPU, AHB and APB busses clocks
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLM = 25;
RCC_OscInitStruct.PLL.PLLN = 320;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
RCC_OscInitStruct.PLL.PLLQ = 7;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
/**Initializes the CPU, AHB and APB busses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
/**Configure the Systick interrupt time
*/
HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);
/**Configure the Systick
*/
HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
/* SysTick_IRQn interrupt configuration */
HAL_NVIC_SetPriority(SysTick_IRQn, 15, 0);
}
/* USER CODE BEGIN 4 */
/* USER CODE END 4 */
/**
* @brief Period elapsed callback in non blocking mode
* @note This function is called when TIM1 interrupt took place, inside
* HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment
* a global variable "uwTick" used as application time base.
* @param htim : TIM handle
* @retval None
*/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
/* USER CODE BEGIN Callback 0 */
/* USER CODE END Callback 0 */
if (htim->Instance == TIM1) {
HAL_IncTick();
}
/* USER CODE BEGIN Callback 1 */
/* USER CODE END Callback 1 */
}
/**
* @brief This function is executed in case of error occurrence.
* @param None
* @retval None
*/
void _Error_Handler(char * file, int line)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
//TODO
while(1)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t* file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
/**
******************************************************************************
* File Name : stm32f4xx_hal_msp.c
* Description : This file provides code for the MSP Initialization
* and de-Initialization codes.
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2018 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
extern void _Error_Handler(char *, int);
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/**
* Initializes the Global MSP.
*/
void HAL_MspInit(void)
{
/* USER CODE BEGIN MspInit 0 */
/* USER CODE END MspInit 0 */
HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_4);
/* System interrupt init*/
/* MemoryManagement_IRQn interrupt configuration */
HAL_NVIC_SetPriority(MemoryManagement_IRQn, 0, 0);
/* BusFault_IRQn interrupt configuration */
HAL_NVIC_SetPriority(BusFault_IRQn, 0, 0);
/* UsageFault_IRQn interrupt configuration */
HAL_NVIC_SetPriority(UsageFault_IRQn, 0, 0);
/* SVCall_IRQn interrupt configuration */
HAL_NVIC_SetPriority(SVCall_IRQn, 0, 0);
/* DebugMonitor_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DebugMonitor_IRQn, 0, 0);
/* PendSV_IRQn interrupt configuration */
HAL_NVIC_SetPriority(PendSV_IRQn, 15, 0);
/* SysTick_IRQn interrupt configuration */
HAL_NVIC_SetPriority(SysTick_IRQn, 15, 0);
/* USER CODE BEGIN MspInit 1 */
/* USER CODE END MspInit 1 */
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
/**
******************************************************************************
* @file stm32f4xx_hal_timebase_TIM.c
* @brief HAL time base based on the hardware TIM.
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2018 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include "stm32f4xx_hal_tim.h"
/** @addtogroup STM32F7xx_HAL_Examples
* @{
*/
/** @addtogroup HAL_TimeBase
* @{
*/
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
TIM_HandleTypeDef htim1;
uint32_t uwIncrementState = 0;
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
/**
* @brief This function configures the TIM1 as a time base source.
* The time source is configured to have 1ms time base with a dedicated
* Tick interrupt priority.
* @note This function is called automatically at the beginning of program after
* reset by HAL_Init() or at any time when clock is configured, by HAL_RCC_ClockConfig().
* @param TickPriority: Tick interrupt priorty.
* @retval HAL status
*/
HAL_StatusTypeDef HAL_InitTick(uint32_t TickPriority)
{
RCC_ClkInitTypeDef clkconfig;
uint32_t uwTimclock = 0;
uint32_t uwPrescalerValue = 0;
uint32_t pFLatency;
/*Configure the TIM1 IRQ priority */
HAL_NVIC_SetPriority(TIM1_UP_TIM10_IRQn, TickPriority ,0);
/* Enable the TIM1 global Interrupt */
HAL_NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);
/* Enable TIM1 clock */
__HAL_RCC_TIM1_CLK_ENABLE();
/* Get clock configuration */
HAL_RCC_GetClockConfig(&clkconfig, &pFLatency);
/* Compute TIM1 clock */
uwTimclock = 2*HAL_RCC_GetPCLK2Freq();
/* Compute the prescaler value to have TIM1 counter clock equal to 1MHz */
uwPrescalerValue = (uint32_t) ((uwTimclock / 1000000) - 1);
/* Initialize TIM1 */
htim1.Instance = TIM1;
/* Initialize TIMx peripheral as follow:
+ Period = [(TIM1CLK/1000) - 1]. to have a (1/1000) s time base.
+ Prescaler = (uwTimclock/1000000 - 1) to have a 1MHz counter clock.
+ ClockDivision = 0
+ Counter direction = Up
*/
htim1.Init.Period = (1000000 / 1000) - 1;
htim1.Init.Prescaler = uwPrescalerValue;
htim1.Init.ClockDivision = 0;
htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
if(HAL_TIM_Base_Init(&htim1) == HAL_OK)
{
/* Start the TIM time Base generation in interrupt mode */
return HAL_TIM_Base_Start_IT(&htim1);
}
/* Return function status */
return HAL_ERROR;
}
/**
* @brief Suspend Tick increment.
* @note Disable the tick increment by disabling TIM1 update interrupt.
* @param None
* @retval None
*/
void HAL_SuspendTick(void)
{
/* Disable TIM1 update Interrupt */
__HAL_TIM_DISABLE_IT(&htim1, TIM_IT_UPDATE);
}
/**
* @brief Resume Tick increment.
* @note Enable the tick increment by Enabling TIM1 update interrupt.
* @param None
* @retval None
*/
void HAL_ResumeTick(void)
{
/* Enable TIM1 Update interrupt */
__HAL_TIM_ENABLE_IT(&htim1, TIM_IT_UPDATE);
}
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
/**
******************************************************************************
* @file stm32f4xx_it.c
* @brief Interrupt Service Routines.
******************************************************************************
*
* COPYRIGHT(c) 2018 STMicroelectronics
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include "stm32f4xx.h"
#include "stm32f4xx_it.h"
#include "cmsis_os.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/* External variables --------------------------------------------------------*/
extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;
extern DAC_HandleTypeDef hdac;
extern UART_HandleTypeDef huart1;
extern TIM_HandleTypeDef htim1;
/******************************************************************************/
/* Cortex-M4 Processor Interruption and Exception Handlers */
/******************************************************************************/
/**
* @brief This function handles System tick timer.
*/
void SysTick_Handler(void)
{
/* USER CODE BEGIN SysTick_IRQn 0 */
/* USER CODE END SysTick_IRQn 0 */
osSystickHandler();
/* USER CODE BEGIN SysTick_IRQn 1 */
/* USER CODE END SysTick_IRQn 1 */
}
/******************************************************************************/
/* STM32F4xx Peripheral Interrupt Handlers */
/* Add here the Interrupt Handlers for the used peripherals. */
/* For the available peripheral interrupt handler names, */
/* please refer to the startup file (startup_stm32f4xx.s). */
/******************************************************************************/
/**
* @brief This function handles EXTI line0 interrupt.
*/
void EXTI0_IRQHandler(void)
{
/* USER CODE BEGIN EXTI0_IRQn 0 */
/* USER CODE END EXTI0_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_0);
/* USER CODE BEGIN EXTI0_IRQn 1 */
/* USER CODE END EXTI0_IRQn 1 */
}
/**
* @brief This function handles CAN1 TX interrupts.
*/
void CAN1_TX_IRQHandler(void)
{
/* USER CODE BEGIN CAN1_TX_IRQn 0 */
/* USER CODE END CAN1_TX_IRQn 0 */
HAL_CAN_IRQHandler(&hcan1);
/* USER CODE BEGIN CAN1_TX_IRQn 1 */
/* USER CODE END CAN1_TX_IRQn 1 */
}
/**
* @brief This function handles CAN1 RX0 interrupts.
*/
void CAN1_RX0_IRQHandler(void)
{
/* USER CODE BEGIN CAN1_RX0_IRQn 0 */
/* USER CODE END CAN1_RX0_IRQn 0 */
HAL_CAN_IRQHandler(&hcan1);
/* USER CODE BEGIN CAN1_RX0_IRQn 1 */
/* USER CODE END CAN1_RX0_IRQn 1 */
}
/**
* @brief This function handles CAN1 SCE interrupt.
*/
void CAN1_SCE_IRQHandler(void)
{
/* USER CODE BEGIN CAN1_SCE_IRQn 0 */
/* USER CODE END CAN1_SCE_IRQn 0 */
HAL_CAN_IRQHandler(&hcan1);
/* USER CODE BEGIN CAN1_SCE_IRQn 1 */
/* USER CODE END CAN1_SCE_IRQn 1 */
}
/**
* @brief This function handles TIM1 update interrupt and TIM10 global interrupt.
*/
void TIM1_UP_TIM10_IRQHandler(void)
{
/* USER CODE BEGIN TIM1_UP_TIM10_IRQn 0 */
/* USER CODE END TIM1_UP_TIM10_IRQn 0 */
HAL_TIM_IRQHandler(&htim1);
/* USER CODE BEGIN TIM1_UP_TIM10_IRQn 1 */
/* USER CODE END TIM1_UP_TIM10_IRQn 1 */
}
/**
* @brief This function handles USART1 global interrupt.
*/
void USART1_IRQHandler(void)
{
/* USER CODE BEGIN USART1_IRQn 0 */
/* USER CODE END USART1_IRQn 0 */
HAL_UART_IRQHandler(&huart1);
/* USER CODE BEGIN USART1_IRQn 1 */
/* USER CODE END USART1_IRQn 1 */
}
/**
* @brief This function handles TIM6 global interrupt, DAC1 and DAC2 underrun error interrupts.
*/
void TIM6_DAC_IRQHandler(void)
{
/* USER CODE BEGIN TIM6_DAC_IRQn 0 */
/* USER CODE END TIM6_DAC_IRQn 0 */
HAL_DAC_IRQHandler(&hdac);
/* USER CODE BEGIN TIM6_DAC_IRQn 1 */
/* USER CODE END TIM6_DAC_IRQn 1 */
}
/**
* @brief This function handles CAN2 TX interrupts.
*/
void CAN2_TX_IRQHandler(void)
{
/* USER CODE BEGIN CAN2_TX_IRQn 0 */
/* USER CODE END CAN2_TX_IRQn 0 */
HAL_CAN_IRQHandler(&hcan2);
/* USER CODE BEGIN CAN2_TX_IRQn 1 */
/* USER CODE END CAN2_TX_IRQn 1 */
}
/**
* @brief This function handles CAN2 RX0 interrupts.
*/
void CAN2_RX0_IRQHandler(void)
{
/* USER CODE BEGIN CAN2_RX0_IRQn 0 */
/* USER CODE END CAN2_RX0_IRQn 0 */
HAL_CAN_IRQHandler(&hcan2);
/* USER CODE BEGIN CAN2_RX0_IRQn 1 */
/* USER CODE END CAN2_RX0_IRQn 1 */
}
/**
* @brief This function handles CAN2 SCE interrupt.
*/
void CAN2_SCE_IRQHandler(void)
{
/* USER CODE BEGIN CAN2_SCE_IRQn 0 */
/* USER CODE END CAN2_SCE_IRQn 0 */
HAL_CAN_IRQHandler(&hcan2);
/* USER CODE BEGIN CAN2_SCE_IRQn 1 */
/* USER CODE END CAN2_SCE_IRQn 1 */
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
/**
******************************************************************************
* File Name : USART.c
* Description : This file provides code for the configuration
* of the USART instances.
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2018 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "usart.h"
#include "gpio.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
UART_HandleTypeDef huart1;
/* USART1 init function */
void MX_USART1_UART_Init(void)
{
huart1.Instance = USART1;
huart1.Init.BaudRate = 115200;
huart1.Init.WordLength = UART_WORDLENGTH_8B;
huart1.Init.StopBits = UART_STOPBITS_1;
huart1.Init.Parity = UART_PARITY_NONE;
huart1.Init.Mode = UART_MODE_TX_RX;
huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart1.Init.OverSampling = UART_OVERSAMPLING_16;
if (HAL_UART_Init(&huart1) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
}
void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
{
GPIO_InitTypeDef GPIO_InitStruct;
if(uartHandle->Instance==USART1)
{
/* USER CODE BEGIN USART1_MspInit 0 */
/* USER CODE END USART1_MspInit 0 */
/* USART1 clock enable */
__HAL_RCC_USART1_CLK_ENABLE();
/**USART1 GPIO Configuration
PA9 ------> USART1_TX
PA10 ------> USART1_RX
*/
GPIO_InitStruct.Pin = DEBUG_TX_Pin|DEBUG_RX_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF7_USART1;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/* USART1 interrupt Init */
HAL_NVIC_SetPriority(USART1_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(USART1_IRQn);
/* USER CODE BEGIN USART1_MspInit 1 */
/* USER CODE END USART1_MspInit 1 */
}
}
void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
{
if(uartHandle->Instance==USART1)
{
/* USER CODE BEGIN USART1_MspDeInit 0 */
/* USER CODE END USART1_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_USART1_CLK_DISABLE();
/**USART1 GPIO Configuration
PA9 ------> USART1_TX
PA10 ------> USART1_RX
*/
HAL_GPIO_DeInit(GPIOA, DEBUG_TX_Pin|DEBUG_RX_Pin);
/* USART1 interrupt Deinit */
HAL_NVIC_DisableIRQ(USART1_IRQn);
/* USER CODE BEGIN USART1_MspDeInit 1 */
/* USER CODE END USART1_MspDeInit 1 */
}
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
#include "utils.h"
#include "math.h"
#include "drive.h"
/*
* @brief Converts char array to float
* @param char value[] - char array to be converted (4 bytes)
* @param float *p_result - pointer to result float value
* @retval none
*/
void char2float(char value[], float *p_result) {
*((unsigned char *)(p_result) +3) = value[0];
*((unsigned char *)(p_result) +2) = value[1];
*((unsigned char *)(p_result) +1) = value[2];
*((unsigned char *)(p_result) +0) = value[3];
/*
*((unsigned char *)(p_result) +0) = value[0];
*((unsigned char *)(p_result) +1) = value[1];
*((unsigned char *)(p_result) +2) = value[2];
*((unsigned char *)(p_result) +3) = value[3];*/
}
/*
* @brief Converts m/s to km/h
* @param float speed_ms - speed to be converted in m/s
* @retval float - converted speed in km/h
*/
float convertMStoKMH(float speed_ms) {
float speed;
speed = 3.6f*speed_ms;
return speed;
}
/*
* @brief Converts radians to float value with 0.5 precision
* @param float rad - wheel angle in radians
* @retval float angle - angle value in float
*/
float convertRADtoFLOAT(float rad) {
float angle = 12.5f * rad * 180.0f / 3.14f;
angle = round(2 * angle) / 2.0f;
return angle;
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or sign in to comment