Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/Tools/AP_Periph/AP_Periph.cpp

308 lines
8.3 KiB
C++
Raw Blame History

/*
This program 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 program 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/>.
*/
/*
AP_Periph main firmware
To flash this firmware on Linux use:
st-flash write build/f103-periph/bin/AP_Periph.bin 0x8006000
*/
#include <AP_HAL/AP_HAL.h>
#include "AP_Periph.h"
#include <stdio.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include <AP_HAL_ChibiOS/hwdef/common/stm32_util.h>
#include <AP_HAL_ChibiOS/hwdef/common/watchdog.h>
#endif
extern const AP_HAL::HAL &hal;
AP_Periph_FW periph;
void setup();
void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
void stm32_watchdog_init() {}
void stm32_watchdog_pat() {}
#endif
void setup(void)
{
periph.init();
}
void loop(void)
{
periph.update();
}
static uint32_t start_ms;
/*
declare constant app_descriptor in flash
*/
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
const struct app_descriptor app_descriptor __attribute__((section(".app_descriptor")));
#else
const struct app_descriptor app_descriptor;
#endif
void AP_Periph_FW::init()
{
// always run with watchdog enabled. This should have already been
// setup by the bootloader, but if not then enable now
stm32_watchdog_init();
stm32_watchdog_pat();
hal.uartA->begin(AP_SERIALMANAGER_CONSOLE_BAUD, 32, 32);
hal.uartB->begin(115200, 128, 256);
load_parameters();
stm32_watchdog_pat();
can_start();
serial_manager.init();
stm32_watchdog_pat();
#ifdef HAL_BOARD_AP_PERIPH_ZUBAXGNSS
// setup remapping register for ZubaxGNSS
uint32_t mapr = AFIO->MAPR;
mapr &= ~AFIO_MAPR_SWJ_CFG;
mapr |= AFIO_MAPR_SWJ_CFG_JTAGDISABLE;
AFIO->MAPR = mapr | AFIO_MAPR_CAN_REMAP_REMAP2 | AFIO_MAPR_SPI3_REMAP;
#endif
printf("Booting %08x:%08x %u/%u len=%u 0x%08x\n",
app_descriptor.image_crc1,
app_descriptor.image_crc2,
app_descriptor.version_major, app_descriptor.version_minor,
app_descriptor.image_size,
app_descriptor.git_hash);
if (hal.util->was_watchdog_reset()) {
printf("Reboot after watchdog reset\n");
}
#ifdef HAL_PERIPH_ENABLE_GPS
if (gps.get_type(0) != AP_GPS::GPS_Type::GPS_TYPE_NONE) {
gps.init(serial_manager);
}
#endif
#ifdef HAL_PERIPH_ENABLE_MAG
if (compass.enabled()) {
compass.init();
}
#endif
#ifdef HAL_PERIPH_ENABLE_BARO
baro.init();
#endif
#ifdef HAL_PERIPH_ENABLE_BATTERY
battery.lib.init();
#endif
#ifdef HAL_PERIPH_NEOPIXEL_COUNT
hal.rcout->init();
hal.rcout->set_serial_led_num_LEDs(HAL_PERIPH_NEOPIXEL_CHAN, AP_HAL::RCOutput::MODE_NEOPIXEL);
#endif
#ifdef HAL_PERIPH_ENABLE_ADSB
adsb_init();
#endif
#ifdef HAL_PERIPH_ENABLE_AIRSPEED
if (airspeed.enabled()) {
airspeed.init();
}
#endif
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
if (rangefinder.get_type(0) != RangeFinder::Type::NONE) {
const uint8_t sernum = 3; // uartB
hal.uartB->begin(g.rangefinder_baud);
serial_manager.set_protocol_and_baud(sernum, AP_SerialManager::SerialProtocol_Rangefinder, g.rangefinder_baud);
rangefinder.init(ROTATION_NONE);
}
#endif
#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
pwm_hardpoint_init();
#endif
#ifdef HAL_PERIPH_ENABLE_HWESC
hwesc_telem.init(hal.uartB);
#endif
#ifdef HAL_PERIPH_ENABLE_MSP
msp_init(hal.uartD);
#endif
start_ms = AP_HAL::native_millis();
}
#if defined(HAL_PERIPH_NEOPIXEL_COUNT) && HAL_PERIPH_NEOPIXEL_COUNT == 8
/*
rotating rainbow pattern on startup
*/
static void update_rainbow()
{
static bool rainbow_done;
if (rainbow_done) {
return;
}
uint32_t now = AP_HAL::native_millis();
if (now-start_ms > 1500) {
rainbow_done = true;
hal.rcout->set_serial_led_rgb_data(HAL_PERIPH_NEOPIXEL_CHAN, -1, 0, 0, 0);
hal.rcout->serial_led_send(HAL_PERIPH_NEOPIXEL_CHAN);
return;
}
static uint32_t last_update_ms;
const uint8_t step_ms = 30;
if (now - last_update_ms < step_ms) {
return;
}
const struct {
uint8_t red;
uint8_t green;
uint8_t blue;
} rgb_rainbow[] = {
{ 255, 0, 0 },
{ 255, 127, 0 },
{ 255, 255, 0 },
{ 0, 255, 0 },
{ 0, 0, 255 },
{ 75, 0, 130 },
{ 143, 0, 255 },
{ 0, 0, 0 },
};
last_update_ms = now;
static uint8_t step;
const uint8_t nsteps = ARRAY_SIZE(rgb_rainbow);
float brightness = 0.3;
for (uint8_t n=0; n<8; n++) {
uint8_t i = (step + n) % nsteps;
hal.rcout->set_serial_led_rgb_data(HAL_PERIPH_NEOPIXEL_CHAN, n,
rgb_rainbow[i].red*brightness,
rgb_rainbow[i].green*brightness,
rgb_rainbow[i].blue*brightness);
}
step++;
hal.rcout->serial_led_send(HAL_PERIPH_NEOPIXEL_CHAN);
}
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && CH_DBG_ENABLE_STACK_CHECK == TRUE
void AP_Periph_FW::show_stack_free()
{
const uint32_t isr_stack_size = uint32_t((const uint8_t *)&__main_stack_end__ - (const uint8_t *)&__main_stack_base__);
can_printf("ISR %u/%u", stack_free(&__main_stack_base__), isr_stack_size);
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);
}
can_printf("%s STACK=%u/%u\n", tp->name, stack_free(tp->wabase), total_stack);
}
}
#endif
void AP_Periph_FW::update()
{
static uint32_t last_led_ms;
uint32_t now = AP_HAL::native_millis();
if (now - last_led_ms > 1000) {
last_led_ms = now;
#ifdef HAL_GPIO_PIN_LED
palToggleLine(HAL_GPIO_PIN_LED);
#endif
#if 0
#ifdef HAL_PERIPH_ENABLE_GPS
hal.uartA->printf("GPS status: %u\n", (unsigned)gps.status());
#endif
#ifdef HAL_PERIPH_ENABLE_MAG
const Vector3f &field = compass.get_field();
hal.uartA->printf("MAG (%d,%d,%d)\n", int(field.x), int(field.y), int(field.z));
#endif
#ifdef HAL_PERIPH_ENABLE_BARO
hal.uartA->printf("BARO H=%u P=%.2f T=%.2f\n", baro.healthy(), baro.get_pressure(), baro.get_temperature());
#endif
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
hal.uartA->printf("RNG %u %ucm\n", rangefinder.num_sensors(), rangefinder.distance_cm_orient(ROTATION_NONE));
#endif
hal.scheduler->delay(1);
#endif
#ifdef HAL_PERIPH_NEOPIXEL_COUNT
hal.rcout->set_serial_led_num_LEDs(HAL_PERIPH_NEOPIXEL_CHAN, HAL_PERIPH_NEOPIXEL_COUNT, AP_HAL::RCOutput::MODE_NEOPIXEL);
#endif
}
static uint32_t last_error_ms;
const auto &ierr = AP::internalerror();
if (now - last_error_ms > 5000 && ierr.errors()) {
// display internal errors as DEBUG every 5s
last_error_ms = now;
can_printf("IERR 0x%x %u", ierr.errors(), ierr.last_error_line());
}
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && CH_DBG_ENABLE_STACK_CHECK == TRUE
static uint32_t last_debug_ms;
if (g.debug==1 && now - last_debug_ms > 5000) {
last_debug_ms = now;
show_stack_free();
}
#endif
#ifdef HAL_PERIPH_ENABLE_BATTERY
if (now - battery.last_read_ms >= 100) {
// update battery at 10Hz
battery.last_read_ms = now;
battery.lib.read();
}
#endif
can_update();
hal.scheduler->delay(1);
#if defined(HAL_PERIPH_NEOPIXEL_COUNT) && HAL_PERIPH_NEOPIXEL_COUNT == 8
update_rainbow();
#endif
#ifdef HAL_PERIPH_ENABLE_ADSB
adsb_update();
#endif
}
AP_HAL_MAIN();
Reference in New Issue View Git Blame Copy Permalink