mirror of https://github.com/ArduPilot/ardupilot
94 lines
2.4 KiB
C++
94 lines
2.4 KiB
C++
#include <AP_HAL/AP_HAL.h>
|
|
#include "Semaphores.h"
|
|
#include <stdio.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#include "Util.h"
|
|
#include "RCOutput.h"
|
|
|
|
using namespace QURT;
|
|
|
|
extern "C" {
|
|
void *fc_heap_alloc(size_t size);
|
|
void fc_heap_free(void* ptr);
|
|
size_t fc_heap_size(void);
|
|
size_t fc_heap_usage(void);
|
|
}
|
|
|
|
uint32_t Util::available_memory(void)
|
|
{
|
|
return fc_heap_size() - fc_heap_usage();
|
|
}
|
|
|
|
/*
|
|
return state of safety switch, if applicable
|
|
*/
|
|
Util::safety_state Util::safety_switch_state(void)
|
|
{
|
|
const auto *rcout = (QURT::RCOutput *)hal.rcout;
|
|
if (rcout != nullptr && rcout->safety_on) {
|
|
return SAFETY_DISARMED;
|
|
}
|
|
return SAFETY_ARMED;
|
|
}
|
|
|
|
#if ENABLE_HEAP
|
|
void *Util::allocate_heap_memory(size_t size)
|
|
{
|
|
struct heap *new_heap = (struct heap*)malloc(sizeof(struct heap));
|
|
if (new_heap != nullptr) {
|
|
new_heap->max_heap_size = size;
|
|
new_heap->current_heap_usage = 0;
|
|
}
|
|
return (void *)new_heap;
|
|
}
|
|
|
|
void *Util::heap_realloc(void *h, void *ptr, size_t old_size, size_t new_size)
|
|
{
|
|
if (h == nullptr) {
|
|
return nullptr;
|
|
}
|
|
|
|
struct heap *heapp = (struct heap*)h;
|
|
|
|
// extract appropriate headers. We use the old_size from the
|
|
// header not from the caller. We use SITL to catch cases they
|
|
// don't match (which would be a lua bug)
|
|
old_size = 0;
|
|
heap_allocation_header *old_header = nullptr;
|
|
if (ptr != nullptr) {
|
|
old_header = ((heap_allocation_header *)ptr) - 1;
|
|
old_size = old_header->allocation_size;
|
|
}
|
|
|
|
if ((heapp->current_heap_usage + new_size - old_size) > heapp->max_heap_size) {
|
|
// fail the allocation as we don't have the memory. Note that we don't simulate fragmentation
|
|
return nullptr;
|
|
}
|
|
|
|
heapp->current_heap_usage -= old_size;
|
|
if (new_size == 0) {
|
|
free(old_header);
|
|
return nullptr;
|
|
}
|
|
|
|
heap_allocation_header *new_header = (heap_allocation_header *)malloc(new_size + sizeof(heap_allocation_header));
|
|
if (new_header == nullptr) {
|
|
// can't get the new memory, old memory is kept
|
|
return nullptr;
|
|
}
|
|
heapp->current_heap_usage += new_size;
|
|
new_header->allocation_size = new_size;
|
|
void *new_mem = new_header + 1;
|
|
|
|
if (ptr == nullptr) {
|
|
return new_mem;
|
|
}
|
|
memcpy(new_mem, ptr, old_size > new_size ? new_size : old_size);
|
|
free(old_header);
|
|
return new_mem;
|
|
}
|
|
|
|
#endif // ENABLE_HEAP
|