/**
* @brief Creates a new thread allocating the memory from the heap.
* @pre The configuration options @p CH_CFG_USE_DYNAMIC and
* @p CH_CFG_USE_HEAP must be enabled in order to use this function.
* @note A thread can terminate by calling @p chThdExit() or by simply
* returning from its main function.
* @note The memory allocated for the thread is not released when the thread
* terminates but when a @p chThdWait() is performed.
*
* @param[in] heapp heap from which allocate the memory or @p NULL for the
* default heap
* @param[in] size size of the working area to be allocated
* @param[in] prio the priority level for the new thread
* @param[in] pf the thread function
* @param[in] arg an argument passed to the thread function. It can be
* @p NULL.
* @return The pointer to the @p thread_t structure allocated for
* the thread into the working space area.
* @retval NULL if the memory cannot be allocated.
*
* @api
*/
thread_t *chThdCreateFromHeap(memory_heap_t *heapp, size_t size,
tprio_t prio, tfunc_t pf, void *arg) {
void *wsp;
thread_t *tp;
wsp = chHeapAlloc(heapp, size);
if (wsp == NULL) {
return NULL;
}
#if CH_DBG_FILL_THREADS == TRUE
_thread_memfill((uint8_t *)wsp,
(uint8_t *)wsp + sizeof(thread_t),
CH_DBG_THREAD_FILL_VALUE);
_thread_memfill((uint8_t *)wsp + sizeof(thread_t),
(uint8_t *)wsp + size,
CH_DBG_STACK_FILL_VALUE);
#endif
chSysLock();
tp = chThdCreateI(wsp, size, prio, pf, arg);
tp->p_flags = CH_FLAG_MODE_HEAP;
chSchWakeupS(tp, MSG_OK);
chSysUnlock();
return tp;
}
static dyn_element_t *dyn_create_object_heap(const char *name,
dyn_list_t *dlp,
size_t size) {
dyn_element_t *dep;
chDbgCheck(name != NULL);
/* Checking if an object with this name has already been created.*/
dep = dyn_list_find(name, dlp);
if (dep != NULL) {
return NULL;
}
/* Allocating space for the new buffer object.*/
/*lint -save -e668 [] Lint is confused by the above chDbgCheck() and
incorrectly assumes that strncpy() could receive a NULL pointer.*/
dep = (dyn_element_t *)chHeapAlloc(NULL, size);
if (dep == NULL) {
return NULL;
}
/* Initializing object list element.*/
strncpy(dep->name, name, CH_CFG_FACTORY_MAX_NAMES_LENGTH);
/*lint -restore*/
dep->refs = (ucnt_t)1;
dep->next = dlp->next;
/* Updating factory list.*/
dlp->next = dep;
return dep;
}
static void dyn1_execute(void) {
size_t n, sz;
void *p1;
tprio_t prio = chThdGetPriority();
(void)chHeapStatus(&heap1, &sz);
/* Starting threads from the heap. */
threads[0] = chThdCreateFromHeap(&heap1, THD_WA_SIZE(THREADS_STACK_SIZE),
prio-1, thread, "A");
threads[1] = chThdCreateFromHeap(&heap1, THD_WA_SIZE(THREADS_STACK_SIZE),
prio-2, thread, "B");
/* Allocating the whole heap in order to make the thread creation fail.*/
(void)chHeapStatus(&heap1, &n);
p1 = chHeapAlloc(&heap1, n);
threads[2] = chThdCreateFromHeap(&heap1, THD_WA_SIZE(THREADS_STACK_SIZE),
prio-3, thread, "C");
chHeapFree(p1);
test_assert(1, (threads[0] != NULL) &&
(threads[1] != NULL) &&
(threads[2] == NULL) &&
(threads[3] == NULL) &&
(threads[4] == NULL),
"thread creation failed");
/* Claiming the memory from terminated threads. */
test_wait_threads();
test_assert_sequence(2, "AB");
/* Heap status checked again.*/
test_assert(3, chHeapStatus(&heap1, &n) == 1, "heap fragmented");
test_assert(4, n == sz, "heap size changed");
}
/**
* @brief Creates a new thread allocating the memory from the heap.
* @pre The configuration options @p CH_USE_DYNAMIC and @p CH_USE_HEAP
* must be enabled in order to use this function.
* @note A thread can terminate by calling @p chThdExit() or by simply
* returning from its main function.
* @note The memory allocated for the thread is not released when the thread
* terminates but when a @p chThdWait() is performed.
*
* @param[in] heapp heap from which allocate the memory or @p NULL for the
* default heap
* @param[in] size size of the working area to be allocated
* @param[in] prio the priority level for the new thread
* @param[in] pf the thread function
* @param[in] arg an argument passed to the thread function. It can be
* @p NULL.
* @return The pointer to the @p Thread structure allocated for
* the thread into the working space area.
* @retval NULL if the memory cannot be allocated.
*
* @api
*/
Thread *chThdCreateFromHeap(MemoryHeap *heapp, size_t size,
tprio_t prio, tfunc_t pf, void *arg) {
void *wsp;
Thread *tp;
wsp = chHeapAlloc(heapp, size);
if (wsp == NULL)
return NULL;
#if CH_DBG_FILL_THREADS
_thread_memfill((uint8_t *)wsp,
(uint8_t *)wsp + sizeof(Thread),
CH_THREAD_FILL_VALUE);
_thread_memfill((uint8_t *)wsp + sizeof(Thread),
(uint8_t *)wsp + size,
CH_STACK_FILL_VALUE);
#endif
chSysLock();
tp = chThdCreateI(wsp, size, prio, pf, arg);
tp->p_flags = THD_MEM_MODE_HEAP;
chSchWakeupS(tp, RDY_OK);
chSysUnlock();
return tp;
}
void initWithSizeAndIncRate(DeviceList* const list, int size, int rate)
{
list->size = size;
list->increment_rate = rate;
list->elements = (Element*) chHeapAlloc( NULL, list->size * sizeof(Element));
chprintf((BaseSequentialStream *)&itm_port, "list->elements: %i\n", *list->elements);
list->current = -1;
}
/**
* @brief Alocate "sz" bytes from heap
*/
void* mangoPort_malloc(uint32_t sz){
#ifdef MANGO_OS_ENV__UNIX
return malloc(sz);
#endif
#ifdef MANGO_OS_ENV__CHIBIOS
return chHeapAlloc(NULL, sz);
#endif
}
pidController *
PidControllerInit( float Kp, float Ki, float Kd, tVexSensors port, int16_t sensor_reverse )
{
pidController *p;
if( nextPidControllerPtr == MAX_PID )
return(NULL);
#ifndef PIDLIB_USE_DYNAMIC
p = (pidController *)&_pidControllers[ nextPidControllerPtr++ ];
#else
p = chHeapAlloc( NULL, sizeof( pidController ) );
#endif
// pid constants
p->Kp = Kp;
p->Ki = Ki;
p->Kd = Kd;
p->Kbias = 0.0;
// zero out working variables
p->error = 0;
p->last_error = 0;
p->integral = 0;
p->derivative = 0;
p->drive = 0.0;
p->drive_cmd = 0;
if(Ki != 0)
p->integral_limit = (PIDLIB_INTEGRAL_DRIVE_MAX / Ki);
else
p->integral_limit = 0;
p->error_threshold = 10;
// sensor port
p->sensor_port = port;
p->sensor_reverse = sensor_reverse;
p->sensor_value = 0;
p->target_value = 0;
// We need a valid sensor for pid control, pot or encoder
// different sensor use in ConVEX, always enable for now
if( 1 )
p->enabled = 1;
else
p->enabled = 0;
PidControllerMakeLut();
return(p);
}
err_t sys_mbox_new(sys_mbox_t *mbox, int size) {
*mbox = chHeapAlloc(NULL, sizeof(mailbox_t) + sizeof(msg_t) * size);
if (*mbox == 0) {
SYS_STATS_INC(mbox.err);
return ERR_MEM;
}
else {
chMBObjectInit(*mbox, (void *)(((uint8_t *)*mbox) + sizeof(mailbox_t)), size);
SYS_STATS_INC(mbox.used);
return ERR_OK;
}
}
err_t sys_sem_new(sys_sem_t *sem, u8_t count) {
*sem = chHeapAlloc(NULL, sizeof(semaphore_t));
if (*sem == 0) {
SYS_STATS_INC(sem.err);
return ERR_MEM;
}
else {
chSemObjectInit(*sem, (cnt_t)count);
SYS_STATS_INC_USED(sem);
return ERR_OK;
}
}
void *osAllocMem(size_t size)
{
void *p;
//Allocate a memory block
p = chHeapAlloc(NULL, size);
//Debug message
TRACE_DEBUG("Allocating %" PRIuSIZE " bytes at 0x%08" PRIXPTR "\r\n", size, (uintptr_t) p);
//Return a pointer to the newly allocated memory block
return p;
}
int
vexAudioInitChipToneSong( int len )
{
int i;
vsl_ct *p;
// already have an array ?
if(VSL_ctarray != NULL)
{
if( ctSize < len )
{
chHeapFree(VSL_ctarray);
VSL_ctarray = NULL;
}
}
// allocate some storage for the chiptone array if we need to
if(VSL_ctarray == NULL)
VSL_ctarray = (vsl_ct *)chHeapAlloc( NULL, sizeof(vsl_ct) * (len + 1) );
// clear everything
ctSetPtr = 0;
ctSize = 0;
ctPlayPtr = 0;
ctRepeat = 0;
// clear array
if( VSL_ctarray != NULL )
{
p = VSL_ctarray;
for(i=0;i<len;i++,p++)
{
p->freq = 0;
#ifdef VSL_AMP_PER_TONE
p->amplitude = 0;
#endif
p->timems = 0;
}
ctSize = len;
return(1); // success
}
else
return(0); // error
}
caddr_t _sbrk(struct _reent *r, int incr)
{
#if CH_CFG_USE_MEMCORE
void *p;
chDbgCheck(incr >= 0);
p = chHeapAlloc(NULL, (size_t)incr);
if (p == NULL) {
__errno_r(r) = ENOMEM;
return (caddr_t)-1;
}
return (caddr_t)p;
#else
(void)incr;
__errno_r(r) = ENOMEM;
return (caddr_t)-1;
#endif
}
static int detect_geos(void)
{
int i, j;
uint8_t *gnss_buffer = chHeapAlloc(NULL, 1024);
int detected = 0;
palSetPad(IOPORT1, PIOA_GPS_NRST);
chThdSleepMilliseconds(100);
k2_usart1_geos();
for (i = 0; i < 20; i++) {
int t = sdReadTimeout(&SD1, gnss_buffer, 1024, 250);
if (t > 0) {
for (j = 0; j < t; j++) {
if (packet_header(gnss_buffer[j],
(uint8_t *) "PSGG", 4))
detected = 1;
}
}
}
chHeapFree(gnss_buffer);
return detected;
}
void packet_detector_geos(int c)
{
static int state = 0;
static int packetnum = 0;
static int packetsize = 0;
static int packet_count = 0;
int crc;
static struct packet_msg *packet = NULL;
if (state != 0)
crc = geos_crc(c, 0);
switch(state) {
case 0:
if (packet_header(c, (uint8_t *)"PSGG", 4)) {
state = 1;
geos_crc('P', 1);
geos_crc('S', 0);
geos_crc('G', 0);
geos_crc('G', 0);
}
break;
case 1:
packetnum = c;
state = 2;
break;
case 2:
packetnum |= (c << 8);
state = 3;
break;
case 3:
packetsize = c;
state = 4;
break;
case 4:
packetsize |= (c << 8);
state = 5;
packet = chHeapAlloc(NULL,
sizeof(struct packet_msg) + packetsize * 4);
if (!packet) {
printf("Drop\r\n");
state = 0;
break;
}
packet->len = packetsize *4;
packet->id = packetnum;
break;
case 5:
case 6:
case 7:
case 8:
if (packet) {
if (packet->len > packet_count) {
packet->data[packet_count] = c;
packet_count++;
}
}
if (state == 8) {
packetsize--;
if(!packetsize)
state = 100;
else
state = 5;
} else
state++;
break;
/* CRC */
case 100:
case 101:
case 102:
state++;
break;
case 103:
state = 0;
if(crc) {
printf("b\r\n");
chHeapFree(packet);
} else {
process_packet(packet);
}
packet_count = 0;
break;
}
}
static void heap1_execute(void) {
void *p1, *p2, *p3;
size_t n, sz;
/* Unrelated, for coverage only.*/
(void)chCoreStatus();
/*
* Test on the default heap in order to cover the core allocator at
* least one time.
*/
(void)chHeapStatus(NULL, &sz);
p1 = chHeapAlloc(NULL, SIZE);
test_assert(1, p1 != NULL, "allocation failed");
chHeapFree(p1);
p1 = chHeapAlloc(NULL, (size_t)-256);
test_assert(2, p1 == NULL, "allocation not failed");
/* Initial local heap state.*/
(void)chHeapStatus(&test_heap, &sz);
/* Same order.*/
p1 = chHeapAlloc(&test_heap, SIZE);
p2 = chHeapAlloc(&test_heap, SIZE);
p3 = chHeapAlloc(&test_heap, SIZE);
chHeapFree(p1); /* Does not merge.*/
chHeapFree(p2); /* Merges backward.*/
chHeapFree(p3); /* Merges both sides.*/
test_assert(3, chHeapStatus(&test_heap, &n) == 1, "heap fragmented");
/* Reverse order.*/
p1 = chHeapAlloc(&test_heap, SIZE);
p2 = chHeapAlloc(&test_heap, SIZE);
p3 = chHeapAlloc(&test_heap, SIZE);
chHeapFree(p3); /* Merges forward.*/
chHeapFree(p2); /* Merges forward.*/
chHeapFree(p1); /* Merges forward.*/
test_assert(4, chHeapStatus(&test_heap, &n) == 1, "heap fragmented");
/* Small fragments handling.*/
p1 = chHeapAlloc(&test_heap, SIZE + 1);
p2 = chHeapAlloc(&test_heap, SIZE);
chHeapFree(p1);
test_assert(5, chHeapStatus(&test_heap, &n) == 2, "invalid state");
p1 = chHeapAlloc(&test_heap, SIZE);
/* Note, the first situation happens when the alignment size is smaller
than the header size, the second in the other cases.*/
test_assert(6, (chHeapStatus(&test_heap, &n) == 1) ||
(chHeapStatus(&test_heap, &n) == 2), "heap fragmented");
chHeapFree(p2);
chHeapFree(p1);
test_assert(7, chHeapStatus(&test_heap, &n) == 1, "heap fragmented");
/* Skip fragment handling.*/
p1 = chHeapAlloc(&test_heap, SIZE);
p2 = chHeapAlloc(&test_heap, SIZE);
chHeapFree(p1);
test_assert(8, chHeapStatus(&test_heap, &n) == 2, "invalid state");
p1 = chHeapAlloc(&test_heap, SIZE * 2); /* Skips first fragment.*/
chHeapFree(p1);
chHeapFree(p2);
test_assert(9, chHeapStatus(&test_heap, &n) == 1, "heap fragmented");
/* Allocate all handling.*/
(void)chHeapStatus(&test_heap, &n);
p1 = chHeapAlloc(&test_heap, n);
test_assert(10, chHeapStatus(&test_heap, &n) == 0, "not empty");
chHeapFree(p1);
test_assert(11, chHeapStatus(&test_heap, &n) == 1, "heap fragmented");
test_assert(12, n == sz, "size changed");
}
/* See documentation in header file. */
int
ini_parse_file(FIL* file, int
(*handler)(void*, const char*, const char*, const char*), void* user)
{
/* Uses a fair bit of stack (use heap instead if you need to) */
#if INI_USE_STACK
char line[INI_MAX_LINE];
#else
char* line;
#endif
char section[MAX_SECTION] = "";
char prev_name[MAX_NAME] = "";
char* start;
char* end;
char* name;
char* value;
int lineno = 0;
int error = 0;
#if !INI_USE_STACK
line = (char*) chHeapAlloc(NULL, INI_MAX_LINE);
if (!line)
{
return -2;
}
#endif
/* Scan through file line by line */
while (f_gets(line, INI_MAX_LINE, file) != NULL)
{
lineno++;
start = line;
#if INI_ALLOW_BOM
if (lineno == 1 && (unsigned char) start[0] == 0xEF
&& (unsigned char) start[1] == 0xBB
&& (unsigned char) start[2] == 0xBF)
{
start += 3;
}
#endif
start = lskip(rstrip(start));
if (*start == ';' || *start == '#')
{
/* Per Python ConfigParser, allow '#' comments at start of line */
}
#if INI_ALLOW_MULTILINE
else if (*prev_name && *start && start > line)
{
/* Non-black line with leading whitespace, treat as continuation
of previous name's value (as per Python ConfigParser). */
if (!handler(user, section, prev_name, start) && !error)
error = lineno;
}
#endif
else if (*start == '[')
{
/* A "[section]" line */
end = find_char_or_comment(start + 1, ']');
if (*end == ']')
{
*end = '\0';
strncpy0(section, start + 1, sizeof(section));
*prev_name = '\0';
}
else if (!error)
{
/* No ']' found on section line */
error = lineno;
}
}
else if (*start && *start != ';')
{
/* Not a comment, must be a name[=:]value pair */
end = find_char_or_comment(start, '=');
if (*end != '=')
{
end = find_char_or_comment(start, ':');
}
if (*end == '=' || *end == ':')
{
*end = '\0';
name = rstrip(start);
value = lskip(end + 1);
end = find_char_or_comment(value, '\0');
if (*end == ';')
*end = '\0';
rstrip(value);
/* Valid name[=:]value pair found, call handler */
strncpy0(prev_name, name, sizeof(prev_name));
if (!handler(user, section, name, value) && !error)
error = lineno;
}
else if (!error)
{
/* No '=' or ':' found on name[=:]value line */
error = lineno;
}
}
}
#if !INI_USE_STACK
chHeapFree(line);
#endif
return error;
}
void* operator new(size_t size) {
return chHeapAlloc(0x0, size);
}
/* ChibiOS Hooks */
static void *chibios_malloc(size_t sz)
{
return chHeapAlloc(NULL, sz);
}
/*------------------------------------------------------------------------*/
void *ff_memalloc(UINT size) {
return chHeapAlloc(NULL, size);
}
/**
* @brief This is the pressure control thread
* @param void* to a PID Loops configuration
* @retval msg_t status
*/
msg_t Pressure_Thread(void *This_Config) {
/* This thread is passed a pointer to a PID loop configuration */
PID_State Pressure_PID_Controllers[((Pressure_Config_Type*)This_Config)->Number_Setpoints];
memset(Pressure_PID_Controllers,0,((Pressure_Config_Type*)This_Config)->Number_Setpoints*sizeof(PID_State));/* Initialise as zeros */
float* Last_PID_Out=(float*)chHeapAlloc(NULL,sizeof(float)*((Pressure_Config_Type*)This_Config)->Number_Setpoints);/* PID output for interpol */
adcsample_t Pressure_Samples[PRESSURE_SAMPLES],Pressure_Sample;/* Use multiple pressure samples to drive down the noise */
float PID_Out,Pressure;//,step=0.01,sawtooth=0.7;
uint32_t Setpoint=0;
uint8_t Old_Setpoint=0, Previous_Setpoint;
chRegSetThreadName("PID_Pressure");
//palSetGroupMode(GPIOC, PAL_PORT_BIT(5) | PAL_PORT_BIT(4), 0, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOE, 9, PAL_MODE_ALTERNATE(1)); /* Only set the pin as AF output here, so as to avoid solenoid getting driven earlier*/
palSetPadMode(GPIOE, 11, PAL_MODE_ALTERNATE(1)); /* Experimental servo output here */
#ifndef USE_SERVO
/*
* Activates the PWM driver
*/
pwmStart(&PWM_Driver_Solenoid, &PWM_Config_Solenoid); /* Have to define the timer to use for PWM_Driver in hardware config */
/*
* Set the solenoid PWM to off
*/
pwmEnableChannel(&PWM_Driver_Solenoid, (pwmchannel_t)PWM_CHANNEL_SOLENOID, (pwmcnt_t)0);
#else
/*
* Activates the experimental servo driver
*/
pwmStart(&PWM_Driver_Servo, &PWM_Config_Servo); /* Have to define the timer to use for PWM_Driver in hardware config */
#endif
/*
* Activates the ADC2 driver *and the thermal sensor*.
*/
adcStart(&ADCD2, NULL);
//adcSTM32EnableTSVREFE();
/*
/ Now we run the sensor offset calibration loop
*/
do {
adcConvert(&ADCD2, &adcgrpcfg1, &Pressure_Sample, 1);/* This function blocks until it has one sample*/
} while(Calibrate_Sensor((uint16_t)Pressure_Sample));
systime_t time = chTimeNow(); /* T0 */
systime_t Interpolation_Timeout = time; /* Set to T0 to show there is no current interpolation */
/* Loop for the pressure control thread */
while(TRUE) {
/*
* Linear conversion.
*/
adcConvert(&ADCD2, &adcgrpcfg1, Pressure_Samples, PRESSURE_SAMPLES);/* This function blocks until it has the samples via DMA*/
/*
/ Now we process the data and apply the PID controller - we use a median filter to take out the non guassian noise
*/
Pressure_Sample = quick_select(Pressure_Samples, PRESSURE_SAMPLES);
Pressure = Convert_Pressure((uint16_t)Pressure_Sample);/* Converts to PSI as a float */
/* Retrieve a new setpoint from the setpoint mailbox, only continue if we get it*/
if(chMBFetch(&Pressures_Setpoint, (msg_t*)&Setpoint, TIME_IMMEDIATE) == RDY_OK) {
//Pressure=Run_Pressure_Filter(Pressure);/* Square root raised cosine filter for low pass with minimal lag */
Pressure = Pressure<0?0.0:Pressure; /* A negative pressure is impossible with current hardware setup - disregard*/
Setpoint &= 0x000000FF;
/* The controller is built around an interpolated array of independant PID controllers with seperate setpoints */
if(Setpoint != Old_Setpoint) { /* The setpoint has changed */
Previous_Setpoint = Old_Setpoint;/* This is for use by the interpolator */
Old_Setpoint = Setpoint; /* Store the setpoint */
/* Store the time at which the interpolation to new setpoint completes*/
Interpolation_Timeout = time + (systime_t)( 4.0 / ((Pressure_Config_Type*)This_Config)->Interpolation_Base );
}
if(Interpolation_Timeout > time) { /* If we have an ongoing interpolation - note, operates in tick units */
/* Value goes from 1 to -1 */
float interpol = erff( (float)(Interpolation_Timeout - time) *\
((Pressure_Config_Type*)This_Config)->Interpolation_Base - 2.0 );/* erf function interpolator */
interpol = ( (-interpol + 1.0) / 2.0);/* Interpolation value goes from 0 to 1 */
PID_Out = ( Last_PID_Out[Previous_Setpoint] * (1.0 - interpol) ) + ( Last_PID_Out[Setpoint] * interpol );
Pressure_PID_Controllers[Setpoint].Last_Input = Pressure;/* Make sure the input to next PID controller is continuous */
}
else {
PID_Out = Run_PID_Loop( ((Pressure_Config_Type*)This_Config)->PID_Loop_Config, &Pressure_PID_Controllers[Setpoint],\
(((Pressure_Config_Type*)This_Config)->Setpoints)[Setpoint], \
Pressure, (float)PRESSURE_TIME_INTERVAL/1000.0);/* Run PID */
Last_PID_Out[Setpoint] = PID_Out;/* Store for use by the interpolator */
}
}
else
PID_Out=0; /* So we can turn off the solenoid simply by failing to send Setpoints */
PID_Out=PID_Out>1.0?1.0:PID_Out;
PID_Out=PID_Out<0.0?0.0:PID_Out; /* Enforce range limits on the PID output */
//sawtooth+=step; /* Test code for debugging mechanics with a sawtooth */
//if(sawtooth>=1 || sawtooth<=0.65)
// step=-step;
//PID_Out=sawtooth;
#ifndef USE_SERVO
/*
/ Now we apply the PID output to the PWM based solenoid controller, and feed data into the mailbox output - Note fractional input
*/
pwmEnableChannel(&PWM_Driver_Solenoid, (pwmchannel_t)PWM_CHANNEL_SOLENOID, (pwmcnt_t)PWM_FRACTION_TO_WIDTH(&PWM_Driver_Solenoid, 1000\
, (uint32_t)(1000.0*PID_Out)));
#else
pwmEnableChannel(&PWM_Driver_Servo, (pwmchannel_t)PWM_CHANNEL_SERVO, (pwmcnt_t)PWM_FRACTION_TO_WIDTH(&PWM_Driver_Servo, 10000\
, (uint32_t)(1000.0*(PID_Out+1.0))));
#endif
chMBPost(&Pressures_Reported, *((msg_t*)&Pressure), TIME_IMMEDIATE);/* Non blocking write attempt to the Reported Pressure mailbox FIFO */
/*
/ The Thread is syncronised to system time
*/
time += MS2ST(PRESSURE_TIME_INTERVAL); /* Next deadline */
chThdSleepUntil(time); /* Gives us a thread with regular timing */
}
}
void *operator new[](size_t size) {
return chHeapAlloc(NULL, size);
}
static msg_t GPSThread(void *arg) {
(void)arg;
chRegSetThreadName("gps_thread");
if (gps_data != NULL) {
chHeapFree(gps_data);
}
gps_data = chHeapAlloc(NULL, GPS_CMD_BUF);
size_t gps_bytes_read;
uint16_t i;
if (gps_data == NULL) {
while (TRUE) {
palTogglePad(GPIO_LED_1_PORT, GPIO_LED_1_PIN);
chThdSleepMilliseconds(50);
}
}
sdStart(&GPS_SERIAL, &SD3_Config);
palSetPadMode(GPS_USART_PORT, GPS_USART_TX_PIN, PAL_MODE_ALTERNATE(7));
palSetPadMode(GPS_USART_PORT, GPS_USART_RX_PIN, PAL_MODE_ALTERNATE(7));
gps_reset();
while (TRUE) {
size_t readed_msg_len = 0;
uint8_t res = gps_read_msg(&readed_msg_len);
if (res == E_OK) {
if (check_checksum() != E_OK) {
sdWrite(&SD1, "GPS CHK ERR\r\n", 13);
} else {
if (gps_message_type() == GPS_MESSAGE_GPRMC) {
gps_rmc_state_t state;
//sdWrite(&SD1, gps_data, readed_msg_len);
if (parse_gps_rmc(&state) == E_OK) {
sdWrite(&SD1, "GPS PARSE OK\r\n", 14);
}
} else if (gps_message_type() == GPS_MESSAGE_UNKNOWN) {
sdWrite(&SD1, "GPS MSG UNKNOWN\r\n", 17);
}
}
} else {
gps_data[0] = '0' + res;
sdWrite(&SD1, "GPS ERROR:", 10);
sdWrite(&SD1, gps_data, 1);
gps_reset();
}
sdWrite(&SD1, "\r\n", 2);
//sdWrite(&SD1, "GPS: ", 5);
//while ((gps_bytes_read = sdReadTimeout(&GPS_SERIAL, gps_data, GPS_CMD_BUF, 100)) > 0)
// sdWrite(&SD1, gps_data, gps_bytes_read);
//sdWrite(&SD1, "|||\r\n", 5);
//chThdSleepMilliseconds(500);
}
chHeapFree(gps_data);
}
