2018-01-05 02:19:51 -04:00
|
|
|
/*
|
|
|
|
* This file is free software: you can redistribute it and/or modify it
|
|
|
|
* under the terms of the GNU General Public License as published by the
|
|
|
|
* Free Software Foundation, either version 3 of the License, or
|
|
|
|
* (at your option) any later version.
|
|
|
|
*
|
|
|
|
* This file 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.
|
|
|
|
* See the GNU General Public License for more details.
|
|
|
|
*
|
|
|
|
* You should have received a copy of the GNU General Public License along
|
|
|
|
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
2019-10-20 10:31:12 -03:00
|
|
|
*
|
2018-01-05 02:19:51 -04:00
|
|
|
* Code by Andrew Tridgell and Siddharth Bharat Purohit
|
|
|
|
*/
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#include <AP_Math/AP_Math.h>
|
|
|
|
|
2022-02-20 23:44:56 -04:00
|
|
|
#include <hal.h>
|
2018-01-05 02:19:51 -04:00
|
|
|
#include "Util.h"
|
2018-06-08 13:05:48 -03:00
|
|
|
#include <ch.h>
|
2018-04-14 00:55:03 -03:00
|
|
|
#include "RCOutput.h"
|
2020-12-05 15:16:27 -04:00
|
|
|
#include "UARTDriver.h"
|
2018-06-02 00:27:02 -03:00
|
|
|
#include "hwdef/common/stm32_util.h"
|
2019-04-19 22:25:18 -03:00
|
|
|
#include "hwdef/common/watchdog.h"
|
2018-06-27 02:59:57 -03:00
|
|
|
#include "hwdef/common/flash.h"
|
|
|
|
#include <AP_ROMFS/AP_ROMFS.h>
|
2020-12-30 02:44:19 -04:00
|
|
|
#include <AP_Common/ExpandingString.h>
|
2023-09-19 11:01:09 -03:00
|
|
|
#include <AP_InternalError/AP_InternalError.h>
|
2018-12-27 21:05:06 -04:00
|
|
|
#include "sdcard.h"
|
2020-11-21 12:00:11 -04:00
|
|
|
#include "shared_dma.h"
|
2022-08-15 02:27:28 -03:00
|
|
|
#if defined(HAL_PWM_ALARM) || HAL_DSHOT_ALARM_ENABLED || HAL_CANMANAGER_ENABLED || HAL_USE_PWM == TRUE
|
2021-05-26 12:42:25 -03:00
|
|
|
#include <AP_Notify/AP_Notify.h>
|
|
|
|
#endif
|
2021-01-16 01:01:04 -04:00
|
|
|
#if HAL_ENABLE_SAVE_PERSISTENT_PARAMS
|
|
|
|
#include <AP_InertialSensor/AP_InertialSensor.h>
|
2023-05-25 04:51:46 -03:00
|
|
|
#include <AP_OpenDroneID/AP_OpenDroneID.h>
|
2021-01-16 01:01:04 -04:00
|
|
|
#endif
|
2022-11-24 18:42:27 -04:00
|
|
|
#include <AP_Logger/AP_Logger_config.h>
|
|
|
|
#if HAL_LOGGING_ENABLED
|
2021-09-06 01:46:19 -03:00
|
|
|
#include <AP_Logger/AP_Logger.h>
|
|
|
|
#endif
|
2018-01-05 02:19:51 -04:00
|
|
|
|
|
|
|
#if HAL_WITH_IO_MCU
|
2018-01-05 04:55:01 -04:00
|
|
|
#include <AP_BoardConfig/AP_BoardConfig.h>
|
2018-01-05 02:19:51 -04:00
|
|
|
#include <AP_IOMCU/AP_IOMCU.h>
|
|
|
|
extern AP_IOMCU iomcu;
|
|
|
|
#endif
|
|
|
|
|
2022-09-02 19:45:32 -03:00
|
|
|
#if AP_SIGNED_FIRMWARE && !defined(HAL_BOOTLOADER_BUILD)
|
|
|
|
#include <AP_CheckFirmware/AP_CheckFirmware.h>
|
|
|
|
#endif
|
|
|
|
|
2023-05-25 04:51:46 -03:00
|
|
|
|
2018-01-28 14:31:21 -04:00
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
2018-01-05 02:19:51 -04:00
|
|
|
using namespace ChibiOS;
|
2018-03-06 18:41:03 -04:00
|
|
|
#if CH_CFG_USE_HEAP == TRUE
|
|
|
|
|
2018-01-05 02:19:51 -04:00
|
|
|
/**
|
|
|
|
how much free memory do we have in bytes.
|
|
|
|
*/
|
2018-01-13 00:02:05 -04:00
|
|
|
uint32_t Util::available_memory(void)
|
2018-01-05 02:19:51 -04:00
|
|
|
{
|
2018-01-09 17:18:28 -04:00
|
|
|
// from malloc.c in hwdef
|
|
|
|
return mem_available();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
Special Allocation Routines
|
|
|
|
*/
|
|
|
|
|
2018-01-13 00:02:05 -04:00
|
|
|
void* Util::malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type)
|
2018-01-09 17:18:28 -04:00
|
|
|
{
|
2018-05-31 22:09:05 -03:00
|
|
|
if (mem_type == AP_HAL::Util::MEM_DMA_SAFE) {
|
2018-06-02 00:27:02 -03:00
|
|
|
return malloc_dma(size);
|
|
|
|
} else if (mem_type == AP_HAL::Util::MEM_FAST) {
|
2019-02-17 07:23:34 -04:00
|
|
|
return malloc_fastmem(size);
|
2024-01-04 22:34:37 -04:00
|
|
|
} else if (mem_type == AP_HAL::Util::MEM_FILESYSTEM) {
|
|
|
|
#if defined(STM32H7)
|
|
|
|
return malloc_axi_sram(size);
|
|
|
|
#else
|
|
|
|
return malloc_dma(size);
|
|
|
|
#endif
|
2018-01-09 17:18:28 -04:00
|
|
|
} else {
|
2018-01-16 17:03:26 -04:00
|
|
|
return calloc(1, size);
|
2018-01-09 17:18:28 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-01-13 00:02:05 -04:00
|
|
|
void Util::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type)
|
2018-01-09 17:18:28 -04:00
|
|
|
{
|
|
|
|
if (ptr != NULL) {
|
2020-11-13 01:10:45 -04:00
|
|
|
free(ptr);
|
2018-01-09 17:18:28 -04:00
|
|
|
}
|
|
|
|
}
|
2018-01-05 02:19:51 -04:00
|
|
|
|
|
|
|
|
2024-07-05 20:05:51 -03:00
|
|
|
#if ENABLE_HEAP
|
2020-03-16 04:22:18 -03:00
|
|
|
/*
|
2024-11-17 00:56:45 -04:00
|
|
|
realloc implementation thanks to wolfssl, used by ExpandingString
|
|
|
|
and ExpandingArray
|
2020-03-16 04:22:18 -03:00
|
|
|
*/
|
2024-11-17 00:56:45 -04:00
|
|
|
void *Util::std_realloc(void *addr, uint32_t size)
|
2020-03-16 04:22:18 -03:00
|
|
|
{
|
|
|
|
if (size == 0) {
|
|
|
|
free(addr);
|
|
|
|
return nullptr;
|
|
|
|
}
|
|
|
|
if (addr == nullptr) {
|
|
|
|
return malloc(size);
|
|
|
|
}
|
|
|
|
void *new_mem = malloc(size);
|
|
|
|
if (new_mem != nullptr) {
|
|
|
|
memcpy(new_mem, addr, chHeapGetSize(addr) > size ? size : chHeapGetSize(addr));
|
|
|
|
free(addr);
|
|
|
|
}
|
|
|
|
return new_mem;
|
|
|
|
}
|
|
|
|
|
2018-12-08 22:34:39 -04:00
|
|
|
#endif // ENABLE_HEAP
|
|
|
|
|
2018-03-06 18:41:03 -04:00
|
|
|
#endif // CH_CFG_USE_HEAP
|
|
|
|
|
2018-01-05 02:19:51 -04:00
|
|
|
/*
|
|
|
|
get safety switch state
|
|
|
|
*/
|
2018-01-13 00:02:05 -04:00
|
|
|
Util::safety_state Util::safety_switch_state(void)
|
2018-01-05 02:19:51 -04:00
|
|
|
{
|
2018-04-14 00:55:03 -03:00
|
|
|
#if HAL_USE_PWM == TRUE
|
|
|
|
return ((RCOutput *)hal.rcout)->_safety_switch_state();
|
|
|
|
#else
|
2018-01-05 04:55:01 -04:00
|
|
|
return SAFETY_NONE;
|
2018-04-14 00:55:03 -03:00
|
|
|
#endif
|
2018-01-05 02:19:51 -04:00
|
|
|
}
|
|
|
|
|
2018-02-09 23:55:22 -04:00
|
|
|
#ifdef HAL_PWM_ALARM
|
2018-07-24 22:29:44 -03:00
|
|
|
struct Util::ToneAlarmPwmGroup Util::_toneAlarm_pwm_group = HAL_PWM_ALARM;
|
2022-02-12 17:55:17 -04:00
|
|
|
#elif HAL_USE_PWM == TRUE
|
|
|
|
struct Util::ToneAlarmPwmGroup Util::_toneAlarm_pwm_group = {};
|
2021-04-03 19:00:38 -03:00
|
|
|
#endif
|
|
|
|
|
|
|
|
uint8_t Util::_toneAlarm_types = 0;
|
2018-02-09 23:55:22 -04:00
|
|
|
|
2021-04-03 19:00:38 -03:00
|
|
|
bool Util::toneAlarm_init(uint8_t types)
|
2018-01-28 14:31:21 -04:00
|
|
|
{
|
2021-04-03 19:00:38 -03:00
|
|
|
#ifdef HAL_PWM_ALARM
|
2018-07-24 22:29:44 -03:00
|
|
|
_toneAlarm_pwm_group.pwm_cfg.period = 1000;
|
|
|
|
pwmStart(_toneAlarm_pwm_group.pwm_drv, &_toneAlarm_pwm_group.pwm_cfg);
|
2021-04-03 19:00:38 -03:00
|
|
|
#endif
|
|
|
|
_toneAlarm_types = types;
|
2021-05-26 12:42:25 -03:00
|
|
|
|
2022-08-15 02:27:28 -03:00
|
|
|
#if HAL_USE_PWM != TRUE && !HAL_DSHOT_ALARM_ENABLED && !HAL_CANMANAGER_ENABLED
|
2021-05-26 12:42:25 -03:00
|
|
|
// Nothing to do
|
|
|
|
return false;
|
|
|
|
#else
|
2018-07-24 22:29:44 -03:00
|
|
|
return true;
|
2021-05-26 12:42:25 -03:00
|
|
|
#endif
|
2018-07-18 21:38:13 -03:00
|
|
|
}
|
|
|
|
|
2022-02-12 17:55:17 -04:00
|
|
|
#if HAL_USE_PWM == TRUE
|
|
|
|
bool Util::toneAlarm_init(const PWMConfig& pwm_cfg, PWMDriver* pwm_drv, pwmchannel_t chan, bool active_high)
|
2018-07-27 04:12:26 -03:00
|
|
|
{
|
2021-04-03 19:00:38 -03:00
|
|
|
#ifdef HAL_PWM_ALARM
|
2022-02-12 17:55:17 -04:00
|
|
|
pwmStop(_toneAlarm_pwm_group.pwm_drv);
|
|
|
|
#endif
|
|
|
|
_toneAlarm_pwm_group.pwm_cfg = pwm_cfg;
|
|
|
|
_toneAlarm_pwm_group.pwm_drv = pwm_drv;
|
|
|
|
_toneAlarm_pwm_group.pwm_cfg.period = 1000;
|
|
|
|
_toneAlarm_pwm_group.pwm_cfg.channels[chan].mode = active_high ? PWM_OUTPUT_ACTIVE_HIGH : PWM_OUTPUT_ACTIVE_LOW;
|
|
|
|
_toneAlarm_pwm_group.chan = chan;
|
|
|
|
pwmStart(_toneAlarm_pwm_group.pwm_drv, &_toneAlarm_pwm_group.pwm_cfg);
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
#endif
|
2018-01-28 14:31:21 -04:00
|
|
|
|
2022-02-12 17:55:17 -04:00
|
|
|
void Util::toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms)
|
|
|
|
{
|
|
|
|
#if HAL_USE_PWM == TRUE
|
|
|
|
if (_toneAlarm_pwm_group.pwm_drv != nullptr) {
|
|
|
|
if (is_zero(frequency) || is_zero(volume)) {
|
|
|
|
pwmDisableChannel(_toneAlarm_pwm_group.pwm_drv, _toneAlarm_pwm_group.chan);
|
|
|
|
} else {
|
|
|
|
pwmChangePeriod(_toneAlarm_pwm_group.pwm_drv,
|
|
|
|
roundf(_toneAlarm_pwm_group.pwm_cfg.frequency/frequency));
|
|
|
|
|
|
|
|
pwmEnableChannel(_toneAlarm_pwm_group.pwm_drv, _toneAlarm_pwm_group.chan, roundf(volume*_toneAlarm_pwm_group.pwm_cfg.frequency/frequency)/2);
|
|
|
|
}
|
2018-01-28 14:31:21 -04:00
|
|
|
}
|
2022-02-12 17:55:17 -04:00
|
|
|
#endif // HAL_USE_PWM
|
2022-08-15 02:27:28 -03:00
|
|
|
#if HAL_DSHOT_ALARM_ENABLED
|
2021-04-03 19:00:38 -03:00
|
|
|
// don't play the motors while flying
|
2024-06-19 01:00:07 -03:00
|
|
|
if (!(_toneAlarm_types & uint8_t(AP_Notify::BuzzerType::DSHOT)) || get_soft_armed() || hal.rcout->get_dshot_esc_type() == RCOutput::DSHOT_ESC_NONE) {
|
2021-04-03 19:00:38 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (is_zero(frequency)) { // silence
|
|
|
|
hal.rcout->send_dshot_command(RCOutput::DSHOT_RESET, RCOutput::ALL_CHANNELS, duration_ms);
|
|
|
|
} else if (frequency < 1047) { // C
|
|
|
|
hal.rcout->send_dshot_command(RCOutput::DSHOT_BEEP1, RCOutput::ALL_CHANNELS, duration_ms);
|
|
|
|
} else if (frequency < 1175) { // D
|
|
|
|
hal.rcout->send_dshot_command(RCOutput::DSHOT_BEEP2, RCOutput::ALL_CHANNELS, duration_ms);
|
|
|
|
} else if (frequency < 1319) { // E
|
|
|
|
hal.rcout->send_dshot_command(RCOutput::DSHOT_BEEP3, RCOutput::ALL_CHANNELS, duration_ms);
|
|
|
|
} else if (frequency < 1397) { // F
|
|
|
|
hal.rcout->send_dshot_command(RCOutput::DSHOT_BEEP4, RCOutput::ALL_CHANNELS, duration_ms);
|
|
|
|
} else { // G+
|
|
|
|
hal.rcout->send_dshot_command(RCOutput::DSHOT_BEEP5, RCOutput::ALL_CHANNELS, duration_ms);
|
|
|
|
}
|
2022-08-15 02:27:28 -03:00
|
|
|
#endif // HAL_DSHOT_ALARM_ENABLED
|
2021-04-03 19:00:38 -03:00
|
|
|
}
|
2018-03-06 18:41:03 -04:00
|
|
|
|
2018-06-14 02:31:33 -03:00
|
|
|
/*
|
|
|
|
set HW RTC in UTC microseconds
|
|
|
|
*/
|
|
|
|
void Util::set_hw_rtc(uint64_t time_utc_usec)
|
|
|
|
{
|
|
|
|
stm32_set_utc_usec(time_utc_usec);
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
get system clock in UTC microseconds
|
|
|
|
*/
|
|
|
|
uint64_t Util::get_hw_rtc() const
|
|
|
|
{
|
|
|
|
return stm32_get_utc_usec();
|
|
|
|
}
|
2018-06-27 02:59:57 -03:00
|
|
|
|
2020-03-10 22:21:33 -03:00
|
|
|
#include <GCS_MAVLink/GCS.h>
|
2023-04-19 20:32:32 -03:00
|
|
|
|
|
|
|
#if AP_BOOTLOADER_FLASHING_ENABLED
|
|
|
|
|
2021-08-18 08:42:17 -03:00
|
|
|
#if HAL_GCS_ENABLED
|
2022-11-24 18:42:27 -04:00
|
|
|
#include <GCS_MAVLink/GCS.h>
|
2024-08-07 00:17:21 -03:00
|
|
|
#define Debug(fmt, args ...) do { GCS_SEND_TEXT(MAV_SEVERITY_INFO, fmt, ## args); } while (0)
|
2021-08-18 08:42:17 -03:00
|
|
|
#endif // HAL_GCS_ENABLED
|
|
|
|
|
|
|
|
#ifndef Debug
|
|
|
|
#define Debug(fmt, args ...) do { hal.console->printf(fmt, ## args); } while (0)
|
2020-03-10 22:21:33 -03:00
|
|
|
#endif
|
|
|
|
|
2023-04-19 20:32:32 -03:00
|
|
|
#ifdef HAL_NO_FLASH_SUPPORT
|
|
|
|
#error "Bootloader-flashing enabled but no flashing support"
|
|
|
|
#endif
|
|
|
|
|
2019-10-24 23:15:25 -03:00
|
|
|
Util::FlashBootloader Util::flash_bootloader()
|
2018-06-27 02:59:57 -03:00
|
|
|
{
|
|
|
|
uint32_t fw_size;
|
|
|
|
const char *fw_name = "bootloader.bin";
|
|
|
|
|
2019-05-15 01:09:30 -03:00
|
|
|
EXPECT_DELAY_MS(11000);
|
2019-04-20 03:11:41 -03:00
|
|
|
|
2019-10-23 06:54:10 -03:00
|
|
|
const uint8_t *fw = AP_ROMFS::find_decompress(fw_name, fw_size);
|
2018-06-27 02:59:57 -03:00
|
|
|
if (!fw) {
|
2020-03-10 22:21:33 -03:00
|
|
|
Debug("failed to find %s\n", fw_name);
|
2019-10-24 23:15:25 -03:00
|
|
|
return FlashBootloader::NOT_AVAILABLE;
|
2018-06-27 02:59:57 -03:00
|
|
|
}
|
2022-09-02 19:45:32 -03:00
|
|
|
|
|
|
|
#if AP_SIGNED_FIRMWARE
|
|
|
|
if (!AP_CheckFirmware::check_signed_bootloader(fw, fw_size)) {
|
|
|
|
// don't allow flashing of an unsigned bootloader in a secure
|
|
|
|
// setup. This prevents the easy mistake of leaving an
|
|
|
|
// unsigned bootloader in ROMFS, which would give a trivail
|
|
|
|
// way to bypass signing
|
|
|
|
AP_ROMFS::free(fw);
|
|
|
|
return FlashBootloader::NOT_SIGNED;
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2019-10-25 06:11:59 -03:00
|
|
|
// make sure size is multiple of 32
|
|
|
|
fw_size = (fw_size + 31U) & ~31U;
|
2018-06-27 02:59:57 -03:00
|
|
|
|
2021-01-16 01:01:04 -04:00
|
|
|
bool uptodate = true;
|
2019-03-25 21:21:53 -03:00
|
|
|
const uint32_t addr = hal.flash->getpageaddr(0);
|
2021-01-16 01:01:04 -04:00
|
|
|
|
|
|
|
if (memcmp(fw, (const void*)addr, fw_size) != 0) {
|
|
|
|
uptodate = false;
|
|
|
|
}
|
|
|
|
|
|
|
|
#if HAL_ENABLE_SAVE_PERSISTENT_PARAMS
|
|
|
|
// see if we should store persistent parameters along with the
|
|
|
|
// bootloader. We only do this on boards using a single sector for
|
|
|
|
// the bootloader. The persistent parameters are stored as text at
|
|
|
|
// the end of the sector
|
|
|
|
const int32_t space_available = hal.flash->getpagesize(0) - int32_t(fw_size);
|
|
|
|
ExpandingString persistent_params {}, old_persistent_params {};
|
|
|
|
if (get_persistent_params(persistent_params) &&
|
|
|
|
space_available >= persistent_params.get_length() &&
|
|
|
|
(!load_persistent_params(old_persistent_params) ||
|
|
|
|
strcmp(persistent_params.get_string(),
|
|
|
|
old_persistent_params.get_string()) != 0)) {
|
|
|
|
// persistent parameters have changed, we will update
|
|
|
|
// bootloader to allow storage of the params
|
|
|
|
uptodate = false;
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
|
|
|
if (uptodate) {
|
2020-03-10 22:21:33 -03:00
|
|
|
Debug("Bootloader up-to-date\n");
|
2019-10-23 06:54:10 -03:00
|
|
|
AP_ROMFS::free(fw);
|
2019-10-24 23:15:25 -03:00
|
|
|
return FlashBootloader::NO_CHANGE;
|
2018-06-27 02:59:57 -03:00
|
|
|
}
|
|
|
|
|
2020-03-10 22:21:33 -03:00
|
|
|
Debug("Erasing\n");
|
2019-10-24 23:15:25 -03:00
|
|
|
uint32_t erased_size = 0;
|
|
|
|
uint8_t erase_page = 0;
|
|
|
|
while (erased_size < fw_size) {
|
2019-10-25 06:11:59 -03:00
|
|
|
uint32_t page_size = hal.flash->getpagesize(erase_page);
|
|
|
|
if (page_size == 0) {
|
2019-10-24 23:15:25 -03:00
|
|
|
AP_ROMFS::free(fw);
|
|
|
|
return FlashBootloader::FAIL;
|
|
|
|
}
|
2019-10-25 06:11:59 -03:00
|
|
|
hal.scheduler->expect_delay_ms(1000);
|
|
|
|
if (!hal.flash->erasepage(erase_page)) {
|
2020-03-10 22:21:33 -03:00
|
|
|
Debug("Erase %u failed\n", erase_page);
|
2019-10-24 23:15:25 -03:00
|
|
|
AP_ROMFS::free(fw);
|
|
|
|
return FlashBootloader::FAIL;
|
|
|
|
}
|
|
|
|
erased_size += page_size;
|
|
|
|
erase_page++;
|
2018-06-27 02:59:57 -03:00
|
|
|
}
|
2019-10-24 23:15:25 -03:00
|
|
|
|
2020-03-10 22:21:33 -03:00
|
|
|
Debug("Flashing %s @%08x\n", fw_name, (unsigned int)addr);
|
2018-06-27 02:59:57 -03:00
|
|
|
const uint8_t max_attempts = 10;
|
2019-10-25 06:11:59 -03:00
|
|
|
hal.flash->keep_unlocked(true);
|
2018-06-27 02:59:57 -03:00
|
|
|
for (uint8_t i=0; i<max_attempts; i++) {
|
2019-10-25 06:11:59 -03:00
|
|
|
hal.scheduler->expect_delay_ms(1000);
|
2019-03-25 21:21:53 -03:00
|
|
|
bool ok = hal.flash->write(addr, fw, fw_size);
|
2019-02-03 21:41:45 -04:00
|
|
|
if (!ok) {
|
2020-03-10 22:21:33 -03:00
|
|
|
Debug("Flash failed! (attempt=%u/%u)\n",
|
2018-06-27 02:59:57 -03:00
|
|
|
i+1,
|
|
|
|
max_attempts);
|
2019-10-25 06:11:59 -03:00
|
|
|
hal.scheduler->delay(100);
|
2018-06-27 02:59:57 -03:00
|
|
|
continue;
|
|
|
|
}
|
2020-03-10 22:21:33 -03:00
|
|
|
Debug("Flash OK\n");
|
2021-01-16 01:01:04 -04:00
|
|
|
#if HAL_ENABLE_SAVE_PERSISTENT_PARAMS
|
|
|
|
if (persistent_params.get_length()) {
|
|
|
|
const uint32_t ofs = hal.flash->getpagesize(0) - persistent_params.get_length();
|
|
|
|
hal.flash->write(addr+ofs, persistent_params.get_string(), persistent_params.get_length());
|
|
|
|
}
|
|
|
|
#endif
|
2019-10-25 06:11:59 -03:00
|
|
|
hal.flash->keep_unlocked(false);
|
2019-10-23 06:54:10 -03:00
|
|
|
AP_ROMFS::free(fw);
|
2019-10-24 23:15:25 -03:00
|
|
|
return FlashBootloader::OK;
|
2018-06-27 02:59:57 -03:00
|
|
|
}
|
|
|
|
|
2019-10-25 06:11:59 -03:00
|
|
|
hal.flash->keep_unlocked(false);
|
2020-03-10 22:21:33 -03:00
|
|
|
Debug("Flash failed after %u attempts\n", max_attempts);
|
2019-10-23 06:54:10 -03:00
|
|
|
AP_ROMFS::free(fw);
|
2019-10-24 23:15:25 -03:00
|
|
|
return FlashBootloader::FAIL;
|
2018-06-27 02:59:57 -03:00
|
|
|
}
|
2023-04-19 20:32:32 -03:00
|
|
|
#endif // AP_BOOTLOADER_FLASHING_ENABLED
|
2018-06-27 02:59:57 -03:00
|
|
|
|
2018-06-29 04:54:51 -03:00
|
|
|
/*
|
|
|
|
display system identifer - board type and serial number
|
|
|
|
*/
|
2022-09-18 14:11:05 -03:00
|
|
|
bool Util::get_system_id(char buf[50])
|
2018-06-29 04:54:51 -03:00
|
|
|
{
|
|
|
|
uint8_t serialid[12];
|
2022-09-18 14:11:05 -03:00
|
|
|
char board_name[24];
|
2019-10-20 10:31:12 -03:00
|
|
|
|
2018-06-29 04:54:51 -03:00
|
|
|
memcpy(serialid, (const void *)UDID_START, 12);
|
2022-09-18 14:11:05 -03:00
|
|
|
// avoid board names greater than 23 chars (sizeof includes null char, so allow 24 bytes total)
|
|
|
|
static_assert(sizeof(CHIBIOS_SHORT_BOARD_NAME) <= 24, "CHIBIOS_SHORT_BOARD_NAME must be 23 characters or less");
|
|
|
|
strncpy(board_name, CHIBIOS_SHORT_BOARD_NAME, 23);
|
|
|
|
board_name[23] = 0;
|
2019-10-20 10:31:12 -03:00
|
|
|
|
2018-07-05 18:53:31 -03:00
|
|
|
// this format is chosen to match the format used by HAL_PX4
|
2022-09-18 14:11:05 -03:00
|
|
|
snprintf(buf, 50, "%s %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X",
|
2018-06-29 04:54:51 -03:00
|
|
|
board_name,
|
2019-10-20 10:31:12 -03:00
|
|
|
(unsigned)serialid[3], (unsigned)serialid[2], (unsigned)serialid[1], (unsigned)serialid[0],
|
|
|
|
(unsigned)serialid[7], (unsigned)serialid[6], (unsigned)serialid[5], (unsigned)serialid[4],
|
2018-07-10 01:41:16 -03:00
|
|
|
(unsigned)serialid[11], (unsigned)serialid[10], (unsigned)serialid[9],(unsigned)serialid[8]);
|
2022-09-18 14:11:05 -03:00
|
|
|
buf[49] = 0;
|
2018-06-29 04:54:51 -03:00
|
|
|
return true;
|
|
|
|
}
|
2018-09-28 12:30:48 -03:00
|
|
|
|
|
|
|
bool Util::get_system_id_unformatted(uint8_t buf[], uint8_t &len)
|
|
|
|
{
|
|
|
|
len = MIN(12, len);
|
|
|
|
memcpy(buf, (const void *)UDID_START, len);
|
|
|
|
return true;
|
|
|
|
}
|
2018-12-27 21:05:06 -04:00
|
|
|
|
2019-04-19 22:25:18 -03:00
|
|
|
// return true if the reason for the reboot was a watchdog reset
|
|
|
|
bool Util::was_watchdog_reset() const
|
|
|
|
{
|
|
|
|
return stm32_was_watchdog_reset();
|
|
|
|
}
|
2020-03-27 17:29:12 -03:00
|
|
|
|
2021-05-29 16:47:40 -03:00
|
|
|
#if CH_DBG_ENABLE_STACK_CHECK == TRUE && !defined(HAL_BOOTLOADER_BUILD)
|
2020-03-27 17:29:12 -03:00
|
|
|
/*
|
|
|
|
display stack usage as text buffer for @SYS/threads.txt
|
|
|
|
*/
|
2021-09-04 08:59:15 -03:00
|
|
|
__RAMFUNC__ void Util::thread_info(ExpandingString &str)
|
2020-03-27 17:29:12 -03:00
|
|
|
{
|
2020-12-19 18:43:49 -04:00
|
|
|
#if HAL_ENABLE_THREAD_STATISTICS
|
2022-11-10 12:49:54 -04:00
|
|
|
uint64_t cumulative_cycles = currcore->kernel_stats.m_crit_isr.cumulative;
|
2021-04-13 17:47:36 -03:00
|
|
|
for (thread_t *tp = chRegFirstThread(); tp; tp = chRegNextThread(tp)) {
|
|
|
|
if (tp->stats.best > 0) { // not run
|
|
|
|
cumulative_cycles += (uint64_t)tp->stats.cumulative;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
// a header to allow for machine parsers to determine format
|
|
|
|
const uint32_t isr_stack_size = uint32_t((const uint8_t *)&__main_stack_end__ - (const uint8_t *)&__main_stack_base__);
|
|
|
|
#if HAL_ENABLE_THREAD_STATISTICS
|
|
|
|
str.printf("ThreadsV2\nISR PRI=255 sp=%p STACK=%u/%u LOAD=%4.1f%%\n",
|
|
|
|
&__main_stack_base__,
|
|
|
|
unsigned(stack_free(&__main_stack_base__)),
|
2022-11-10 12:49:54 -04:00
|
|
|
unsigned(isr_stack_size), 100.0f * float(currcore->kernel_stats.m_crit_isr.cumulative) / float(cumulative_cycles));
|
|
|
|
currcore->kernel_stats.m_crit_isr.cumulative = 0U;
|
2020-12-19 18:43:49 -04:00
|
|
|
#else
|
2021-04-13 17:47:36 -03:00
|
|
|
str.printf("ThreadsV2\nISR PRI=255 sp=%p STACK=%u/%u\n",
|
|
|
|
&__main_stack_base__,
|
|
|
|
unsigned(stack_free(&__main_stack_base__)),
|
|
|
|
unsigned(isr_stack_size));
|
2020-12-19 18:43:49 -04:00
|
|
|
#endif
|
2021-04-13 17:47:36 -03:00
|
|
|
for (thread_t *tp = chRegFirstThread(); tp; tp = chRegNextThread(tp)) {
|
|
|
|
uint32_t total_stack;
|
|
|
|
if (tp->wabase == (void*)&__main_thread_stack_base__) {
|
|
|
|
// main thread has its stack separated from the thread context
|
|
|
|
total_stack = uint32_t((const uint8_t *)&__main_thread_stack_end__ - (const uint8_t *)&__main_thread_stack_base__);
|
|
|
|
} else {
|
|
|
|
// all other threads have their thread context pointer
|
|
|
|
// above the stack top
|
|
|
|
total_stack = uint32_t(tp) - uint32_t(tp->wabase);
|
|
|
|
}
|
|
|
|
#if HAL_ENABLE_THREAD_STATISTICS
|
|
|
|
time_measurement_t stats = tp->stats;
|
|
|
|
if (tp->stats.best > 0) { // not run
|
|
|
|
str.printf("%-13.13s PRI=%3u sp=%p STACK=%4u/%4u LOAD=%4.1f%%%s\n",
|
|
|
|
tp->name, unsigned(tp->realprio), tp->wabase,
|
|
|
|
unsigned(stack_free(tp->wabase)), unsigned(total_stack),
|
|
|
|
100.0f * float(stats.cumulative) / float(cumulative_cycles),
|
|
|
|
// more than a loop slice is bad for everyone else, warn on
|
|
|
|
// more than a 200Hz slice so that only the worst offenders are identified
|
|
|
|
// also don't do this for the main or idle threads
|
|
|
|
tp != chThdGetSelfX() && unsigned(RTC2US(STM32_HSECLK, stats.worst)) > 5000
|
|
|
|
&& tp != get_main_thread() && tp->realprio != 1 ? "*" : "");
|
|
|
|
} else {
|
|
|
|
str.printf("%-13.13s PRI=%3u sp=%p STACK=%4u/%4u\n",
|
|
|
|
tp->name, unsigned(tp->realprio), tp->wabase, unsigned(stack_free(tp->wabase)), unsigned(total_stack));
|
|
|
|
}
|
|
|
|
// Giovanni thinks this is dangerous, but we can't get useable data without it
|
|
|
|
if (tp != chThdGetSelfX()) {
|
|
|
|
chTMObjectInit(&tp->stats); // reset counters to zero
|
|
|
|
} else {
|
|
|
|
tp->stats.cumulative = 0U;
|
|
|
|
}
|
|
|
|
#else
|
|
|
|
str.printf("%-13.13s PRI=%3u sp=%p STACK=%u/%u\n",
|
|
|
|
tp->name, unsigned(tp->realprio), tp->wabase,
|
|
|
|
unsigned(stack_free(tp->wabase)), unsigned(total_stack));
|
|
|
|
#endif
|
|
|
|
}
|
2020-03-27 17:29:12 -03:00
|
|
|
}
|
2020-03-29 19:27:34 -03:00
|
|
|
#endif // CH_DBG_ENABLE_STACK_CHECK == TRUE
|
|
|
|
|
2020-11-21 12:00:11 -04:00
|
|
|
#if CH_CFG_USE_SEMAPHORES
|
|
|
|
// request information on dma contention
|
2020-12-30 02:44:19 -04:00
|
|
|
void Util::dma_info(ExpandingString &str)
|
|
|
|
{
|
2022-07-14 04:47:42 -03:00
|
|
|
#if AP_HAL_SHARED_DMA_ENABLED
|
2020-12-30 02:44:19 -04:00
|
|
|
ChibiOS::Shared_DMA::dma_info(str);
|
2021-02-23 00:07:53 -04:00
|
|
|
#endif
|
2020-11-21 12:00:11 -04:00
|
|
|
}
|
|
|
|
#endif
|
2021-01-16 01:01:04 -04:00
|
|
|
|
2021-01-22 06:32:30 -04:00
|
|
|
#if CH_CFG_USE_HEAP == TRUE
|
|
|
|
/*
|
|
|
|
return information on heap usage
|
|
|
|
*/
|
|
|
|
void Util::mem_info(ExpandingString &str)
|
|
|
|
{
|
|
|
|
memory_heap_t *heaps;
|
|
|
|
const struct memory_region *regions;
|
|
|
|
uint8_t num_heaps = malloc_get_heaps(&heaps, ®ions);
|
|
|
|
|
|
|
|
str.printf("MemInfoV1\n");
|
|
|
|
for (uint8_t i=0; i<num_heaps; i++) {
|
|
|
|
size_t totalp=0, largest=0;
|
|
|
|
// get memory available on main heap
|
|
|
|
chHeapStatus(i == 0 ? nullptr : &heaps[i], &totalp, &largest);
|
|
|
|
str.printf("START=0x%08x LEN=%3uk FREE=%6u LRG=%6u TYPE=%1u\n",
|
|
|
|
unsigned(regions[i].address), unsigned(regions[i].size/1024),
|
|
|
|
unsigned(totalp), unsigned(largest), unsigned(regions[i].flags));
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2021-01-16 01:01:04 -04:00
|
|
|
#if HAL_ENABLE_SAVE_PERSISTENT_PARAMS
|
|
|
|
|
|
|
|
static const char *persistent_header = "{{PERSISTENT_START_V1}}\n";
|
|
|
|
|
|
|
|
/*
|
|
|
|
create a set of persistent parameters in string form
|
|
|
|
*/
|
|
|
|
bool Util::get_persistent_params(ExpandingString &str) const
|
|
|
|
{
|
|
|
|
str.printf("%s", persistent_header);
|
|
|
|
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
|
|
|
const auto *ins = AP_InertialSensor::get_singleton();
|
|
|
|
if (ins) {
|
|
|
|
ins->get_persistent_params(str);
|
|
|
|
}
|
2023-05-25 04:51:46 -03:00
|
|
|
#endif
|
|
|
|
#if AP_OPENDRONEID_ENABLED
|
|
|
|
const auto *odid = AP_OpenDroneID::get_singleton();
|
|
|
|
if (odid) {
|
|
|
|
odid->get_persistent_params(str);
|
|
|
|
}
|
2021-01-16 01:01:04 -04:00
|
|
|
#endif
|
|
|
|
if (str.has_failed_allocation() || str.get_length() <= strlen(persistent_header)) {
|
|
|
|
// no data
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
// ensure that the length is a multiple of 32 to meet flash alignment requirements
|
|
|
|
while (!str.has_failed_allocation() && str.get_length() % 32 != 0) {
|
2021-01-16 18:22:29 -04:00
|
|
|
str.append(" ", 1);
|
2021-01-16 01:01:04 -04:00
|
|
|
}
|
|
|
|
return !str.has_failed_allocation();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
load a set of persistent parameters in string form from the bootloader sector
|
|
|
|
*/
|
|
|
|
bool Util::load_persistent_params(ExpandingString &str) const
|
|
|
|
{
|
|
|
|
const uint32_t addr = hal.flash->getpageaddr(0);
|
|
|
|
const uint32_t size = hal.flash->getpagesize(0);
|
|
|
|
const char *s = (const char *)memmem((void*)addr, size,
|
|
|
|
persistent_header,
|
|
|
|
strlen(persistent_header));
|
|
|
|
if (s) {
|
|
|
|
str.append(s, (addr+size) - uint32_t(s));
|
|
|
|
return !str.has_failed_allocation();
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2023-05-26 20:02:50 -03:00
|
|
|
/*
|
|
|
|
get a persistent variable by name,
|
|
|
|
len is the length of the value buffer, and is updated with the length of the value
|
|
|
|
*/
|
|
|
|
bool Util::get_persistent_param_by_name(const char *name, char* value, size_t& len) const
|
|
|
|
{
|
|
|
|
ExpandingString persistent_params {};
|
|
|
|
if (!load_persistent_params(persistent_params)) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
char *s = persistent_params.get_writeable_string();
|
|
|
|
if (s == nullptr) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
char *saveptr;
|
|
|
|
s += strlen(persistent_header);
|
|
|
|
for (char *p = strtok_r(s, "\n", &saveptr);
|
|
|
|
p; p = strtok_r(nullptr, "\n", &saveptr)) {
|
|
|
|
char *eq = strchr(p, int('='));
|
|
|
|
if (eq) {
|
|
|
|
*eq = 0;
|
|
|
|
if (strcmp(p, name) == 0) {
|
|
|
|
// also get the length of the value
|
|
|
|
strncpy(value, eq+1, len);
|
|
|
|
len = strlen(value);
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2021-01-16 01:01:04 -04:00
|
|
|
/*
|
|
|
|
apply persistent parameters from the bootloader sector to AP_Param
|
|
|
|
*/
|
|
|
|
void Util::apply_persistent_params(void) const
|
|
|
|
{
|
|
|
|
ExpandingString str {};
|
|
|
|
if (!load_persistent_params(str)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
char *s = str.get_writeable_string();
|
|
|
|
char *saveptr;
|
|
|
|
s += strlen(persistent_header);
|
|
|
|
uint32_t count = 0;
|
2021-01-21 16:14:33 -04:00
|
|
|
uint32_t errors = 0;
|
2021-01-16 01:01:04 -04:00
|
|
|
for (char *p = strtok_r(s, "\n", &saveptr);
|
|
|
|
p; p = strtok_r(nullptr, "\n", &saveptr)) {
|
|
|
|
char *eq = strchr(p, int('='));
|
|
|
|
if (eq) {
|
|
|
|
*eq = 0;
|
2021-01-21 16:14:33 -04:00
|
|
|
const char *pname = p;
|
2021-09-11 19:26:09 -03:00
|
|
|
const float value = strtof(eq+1, NULL);
|
2021-01-21 16:14:33 -04:00
|
|
|
if (AP_Param::set_default_by_name(pname, value)) {
|
2021-01-16 01:01:04 -04:00
|
|
|
count++;
|
2021-01-21 16:14:33 -04:00
|
|
|
/*
|
|
|
|
we now have a special case for INS_ACC*_ID. To
|
|
|
|
support factory accelerometer calibration we need to
|
|
|
|
do a save() on the ID parameters if they are not
|
|
|
|
already in storage. This is needed as
|
|
|
|
AP_InertialSensor determines if a calibration has
|
|
|
|
been done by whether the IDs are configured in
|
|
|
|
storage
|
|
|
|
*/
|
2024-07-17 04:37:39 -03:00
|
|
|
bool legacy_acc_id = strncmp(pname, "INS_ACC", 7) == 0 &&
|
|
|
|
strcmp(pname+strlen(pname)-3, "_ID") == 0;
|
|
|
|
bool new_acc_id = strncmp(pname, "INS", 3) == 0 &&
|
|
|
|
strcmp(pname+strlen(pname)-6, "ACC_ID") == 0;
|
|
|
|
if (legacy_acc_id || new_acc_id) {
|
2021-01-21 16:14:33 -04:00
|
|
|
enum ap_var_type ptype;
|
|
|
|
AP_Int32 *ap = (AP_Int32 *)AP_Param::find(pname, &ptype);
|
|
|
|
if (ap && ptype == AP_PARAM_INT32) {
|
|
|
|
if (ap->get() != int32_t(value)) {
|
|
|
|
// the accelerometer ID has changed since
|
|
|
|
// this persistent data was saved. Stop
|
|
|
|
// loading persistent parameters as it is
|
|
|
|
// no longer valid for this board. This
|
|
|
|
// can happen if the user has set
|
|
|
|
// parameters to prevent loading of
|
|
|
|
// specific IMU drivers, or if they have
|
|
|
|
// setup an external IMU
|
|
|
|
errors++;
|
|
|
|
break;
|
|
|
|
}
|
2022-05-11 19:04:36 -03:00
|
|
|
if (!ap->configured()) {
|
2021-01-21 16:14:33 -04:00
|
|
|
ap->save();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2021-01-16 01:01:04 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (count) {
|
|
|
|
AP_Param::invalidate_count();
|
2021-01-21 16:14:33 -04:00
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Loaded %u persistent parameters (%u errors)",
|
|
|
|
unsigned(count), unsigned(errors));
|
2021-01-16 01:01:04 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
#endif // HAL_ENABLE_SAVE_PERSISTENT_PARAMS
|
2020-12-05 15:16:27 -04:00
|
|
|
|
2021-06-05 00:51:10 -03:00
|
|
|
#if HAL_WITH_IO_MCU
|
|
|
|
extern ChibiOS::UARTDriver uart_io;
|
|
|
|
#endif
|
|
|
|
|
2022-01-08 06:56:52 -04:00
|
|
|
#if HAL_UART_STATS_ENABLED
|
2020-12-05 15:16:27 -04:00
|
|
|
// request information on uart I/O
|
|
|
|
void Util::uart_info(ExpandingString &str)
|
|
|
|
{
|
2024-03-15 11:20:43 -03:00
|
|
|
// Calculate time since last call
|
|
|
|
const uint32_t now_ms = AP_HAL::millis();
|
2024-04-06 13:11:57 -03:00
|
|
|
const uint32_t dt_ms = now_ms - sys_uart_stats.last_ms;
|
|
|
|
sys_uart_stats.last_ms = now_ms;
|
2024-03-15 11:20:43 -03:00
|
|
|
|
2021-06-05 00:51:10 -03:00
|
|
|
// a header to allow for machine parsers to determine format
|
|
|
|
str.printf("UARTV1\n");
|
|
|
|
for (uint8_t i = 0; i < HAL_UART_NUM_SERIAL_PORTS; i++) {
|
|
|
|
auto *uart = hal.serial(i);
|
|
|
|
if (uart) {
|
|
|
|
str.printf("SERIAL%u ", i);
|
2024-04-06 13:11:57 -03:00
|
|
|
uart->uart_info(str, sys_uart_stats.serial[i], dt_ms);
|
2021-06-05 00:51:10 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
#if HAL_WITH_IO_MCU
|
|
|
|
str.printf("IOMCU ");
|
2024-04-06 13:11:57 -03:00
|
|
|
uart_io.uart_info(str, sys_uart_stats.io, dt_ms);
|
2020-12-05 15:16:27 -04:00
|
|
|
#endif
|
|
|
|
}
|
2024-04-06 13:11:57 -03:00
|
|
|
|
|
|
|
// Log UART message for each serial port
|
|
|
|
#if HAL_LOGGING_ENABLED
|
|
|
|
void Util::uart_log()
|
|
|
|
{
|
|
|
|
// Calculate time since last call
|
|
|
|
const uint32_t now_ms = AP_HAL::millis();
|
|
|
|
const uint32_t dt_ms = now_ms - log_uart_stats.last_ms;
|
|
|
|
log_uart_stats.last_ms = now_ms;
|
|
|
|
|
|
|
|
// Loop over all numbered ports
|
|
|
|
for (uint8_t i = 0; i < HAL_UART_NUM_SERIAL_PORTS; i++) {
|
|
|
|
auto *uart = hal.serial(i);
|
|
|
|
if (uart) {
|
|
|
|
uart->log_stats(i, log_uart_stats.serial[i], dt_ms);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#if HAL_WITH_IO_MCU
|
|
|
|
// Use magic instance 100 for IOMCU
|
|
|
|
uart_io.log_stats(100, log_uart_stats.io, dt_ms);
|
2022-01-08 06:56:52 -04:00
|
|
|
#endif
|
2024-04-06 13:11:57 -03:00
|
|
|
}
|
|
|
|
#endif // HAL_LOGGING_ENABLED
|
|
|
|
#endif // HAL_UART_STATS_ENABLED
|
2021-10-06 02:37:09 -03:00
|
|
|
|
2022-02-10 10:42:33 -04:00
|
|
|
// request information on uart I/O
|
|
|
|
#if HAL_USE_PWM == TRUE
|
|
|
|
void Util::timer_info(ExpandingString &str)
|
|
|
|
{
|
|
|
|
hal.rcout->timer_info(str);
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2021-10-06 02:37:09 -03:00
|
|
|
/**
|
|
|
|
* This method will generate random values with set size. It will fall back to AP_Math's get_random16()
|
|
|
|
* if True RNG fails or enough entropy is not present.
|
|
|
|
*/
|
|
|
|
bool Util::get_random_vals(uint8_t* data, size_t size)
|
|
|
|
{
|
2021-10-06 03:30:38 -03:00
|
|
|
#if HAL_USE_HW_RNG && defined(RNG)
|
2021-10-06 02:37:09 -03:00
|
|
|
size_t true_random_vals = stm32_rand_generate_nonblocking(data, size);
|
2023-07-03 22:18:21 -03:00
|
|
|
if (true_random_vals != size) {
|
2021-10-06 02:37:09 -03:00
|
|
|
if (!(true_random_vals % 2)) {
|
|
|
|
data[true_random_vals] = (uint8_t)(get_random16() & 0xFF);
|
|
|
|
true_random_vals++;
|
|
|
|
}
|
|
|
|
while(true_random_vals < size) {
|
|
|
|
uint16_t val = get_random16();
|
|
|
|
memcpy(&data[true_random_vals], &val, sizeof(uint16_t));
|
|
|
|
true_random_vals+=sizeof(uint16_t);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#else
|
2023-07-03 22:18:21 -03:00
|
|
|
size_t true_random_vals = 0;
|
|
|
|
while(true_random_vals < size) {
|
|
|
|
uint16_t val = get_random16();
|
|
|
|
memcpy(&data[true_random_vals], &val, sizeof(uint16_t));
|
|
|
|
true_random_vals+=sizeof(uint16_t);
|
|
|
|
}
|
|
|
|
if (size % 2) {
|
|
|
|
data[size-1] = get_random16() & 0xFF;
|
|
|
|
}
|
2021-10-06 02:37:09 -03:00
|
|
|
#endif
|
2023-07-03 22:18:21 -03:00
|
|
|
return true;
|
2021-10-06 02:37:09 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
|
|
* This method will generate true random values with set size. This method will block for set amount
|
|
|
|
* of true random numbers to be generated, the timeout specifies the maximum amount of time to wait
|
|
|
|
* for the call to finish.
|
|
|
|
*/
|
|
|
|
bool Util::get_true_random_vals(uint8_t* data, size_t size, uint32_t timeout_us)
|
|
|
|
{
|
2021-10-06 03:30:38 -03:00
|
|
|
#if HAL_USE_HW_RNG && defined(RNG)
|
2021-10-06 02:37:09 -03:00
|
|
|
if (stm32_rand_generate_blocking(data, size, timeout_us)) {
|
|
|
|
return true;
|
|
|
|
} else {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
#else
|
|
|
|
return false;
|
|
|
|
#endif
|
|
|
|
}
|
2021-09-06 01:46:19 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
log info on stack usage. Called at 1Hz by logging thread, logs next
|
|
|
|
thread on each call
|
|
|
|
*/
|
|
|
|
void Util::log_stack_info(void)
|
|
|
|
{
|
2022-11-24 18:42:27 -04:00
|
|
|
#if HAL_LOGGING_ENABLED
|
2021-09-06 01:46:19 -03:00
|
|
|
static thread_t *last_tp;
|
|
|
|
static uint8_t thread_id;
|
|
|
|
thread_t *tp = last_tp;
|
|
|
|
if (tp == nullptr) {
|
|
|
|
tp = chRegFirstThread();
|
|
|
|
thread_id = 0;
|
|
|
|
} else {
|
|
|
|
tp = chRegNextThread(last_tp);
|
|
|
|
thread_id++;
|
|
|
|
}
|
|
|
|
struct log_STAK pkt = {
|
|
|
|
LOG_PACKET_HEADER_INIT(LOG_STAK_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
};
|
|
|
|
if (tp == nullptr) {
|
|
|
|
pkt.thread_id = 255;
|
|
|
|
pkt.priority = 255;
|
|
|
|
const uint32_t isr_stack_size = uint32_t((const uint8_t *)&__main_stack_end__ - (const uint8_t *)&__main_stack_base__);
|
|
|
|
pkt.stack_total = isr_stack_size;
|
|
|
|
pkt.stack_free = stack_free(&__main_stack_base__);
|
|
|
|
strncpy_noterm(pkt.name, "ISR", sizeof(pkt.name));
|
|
|
|
} else {
|
|
|
|
if (tp->wabase == (void*)&__main_thread_stack_base__) {
|
|
|
|
// main thread has its stack separated from the thread context
|
|
|
|
pkt.stack_total = uint32_t((const uint8_t *)&__main_thread_stack_end__ - (const uint8_t *)&__main_thread_stack_base__);
|
|
|
|
} else {
|
|
|
|
// all other threads have their thread context pointer
|
|
|
|
// above the stack top
|
|
|
|
pkt.stack_total = uint32_t(tp) - uint32_t(tp->wabase);
|
|
|
|
}
|
|
|
|
pkt.thread_id = thread_id;
|
|
|
|
pkt.priority = tp->realprio,
|
|
|
|
pkt.stack_free = stack_free(tp->wabase);
|
|
|
|
strncpy_noterm(pkt.name, tp->name, sizeof(pkt.name));
|
|
|
|
}
|
|
|
|
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
last_tp = tp;
|
|
|
|
#endif
|
|
|
|
}
|
2021-10-23 03:34:35 -03:00
|
|
|
|
2022-08-15 04:22:48 -03:00
|
|
|
#if AP_CRASHDUMP_ENABLED
|
2021-11-03 13:48:00 -03:00
|
|
|
size_t Util::last_crash_dump_size() const
|
2021-10-23 03:34:35 -03:00
|
|
|
{
|
|
|
|
// get dump size
|
|
|
|
uint32_t size = stm32_crash_dump_size();
|
2021-11-26 00:50:14 -04:00
|
|
|
char* dump_start = (char*)stm32_crash_dump_addr();
|
2021-10-23 03:34:35 -03:00
|
|
|
if (!(dump_start[0] == 0x63 && dump_start[1] == 0x43)) {
|
|
|
|
// there's no valid Crash Dump
|
2021-11-03 13:48:00 -03:00
|
|
|
return 0;
|
2021-10-23 03:34:35 -03:00
|
|
|
}
|
|
|
|
if (size == 0xFFFFFFFF) {
|
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Crash Dump incomplete, dumping what we got!");
|
2021-11-26 00:50:14 -04:00
|
|
|
size = stm32_crash_dump_max_size();
|
2021-10-23 03:34:35 -03:00
|
|
|
}
|
2021-11-03 13:48:00 -03:00
|
|
|
return size;
|
2021-10-23 03:34:35 -03:00
|
|
|
}
|
2021-11-03 13:48:00 -03:00
|
|
|
|
|
|
|
void* Util::last_crash_dump_ptr() const
|
|
|
|
{
|
|
|
|
if (last_crash_dump_size() == 0) {
|
|
|
|
return nullptr;
|
|
|
|
}
|
2021-11-26 00:50:14 -04:00
|
|
|
return (void*)stm32_crash_dump_addr();
|
2021-11-03 13:48:00 -03:00
|
|
|
}
|
2022-08-15 04:22:48 -03:00
|
|
|
#endif // AP_CRASHDUMP_ENABLED
|
2021-10-24 18:09:18 -03:00
|
|
|
|
2022-08-16 00:10:27 -03:00
|
|
|
#if HAL_ENABLE_DFU_BOOT && !defined(HAL_BOOTLOADER_BUILD)
|
|
|
|
void Util::boot_to_dfu()
|
|
|
|
{
|
|
|
|
hal.util->persistent_data.boot_to_dfu = true;
|
|
|
|
stm32_watchdog_save((uint32_t *)&hal.util->persistent_data, (sizeof(hal.util->persistent_data)+3)/4);
|
2024-03-06 12:33:01 -04:00
|
|
|
hal.scheduler->reboot();
|
2022-08-16 00:10:27 -03:00
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2021-10-24 18:09:18 -03:00
|
|
|
// set armed state
|
|
|
|
void Util::set_soft_armed(const bool b)
|
|
|
|
{
|
|
|
|
AP_HAL::Util::set_soft_armed(b);
|
|
|
|
#ifdef HAL_GPIO_PIN_nARMED
|
|
|
|
palWriteLine(HAL_GPIO_PIN_nARMED, !b);
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|