mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-04 21:23:57 +08:00
Tease the PWM driver out and fix some build issues after cleaning up behind the cpuload pieces.
This commit is contained in:
px4dev
@@ -95,7 +95,7 @@
|
|||||||
PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */
|
PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */
|
||||||
//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */
|
//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */
|
||||||
|
|
||||||
#include <arch/board/up_cpuload.h>
|
#include <systemlib/cpuload.h>
|
||||||
extern struct system_load_s system_load;
|
extern struct system_load_s system_load;
|
||||||
|
|
||||||
/* Decouple update interval and hysteris counters, all depends on intervals */
|
/* Decouple update interval and hysteris counters, all depends on intervals */
|
||||||
|
|||||||
@@ -63,7 +63,6 @@
|
|||||||
#include "px4fmu_internal.h"
|
#include "px4fmu_internal.h"
|
||||||
#include "stm32_uart.h"
|
#include "stm32_uart.h"
|
||||||
|
|
||||||
#include <arch/board/up_cpuload.h>
|
|
||||||
#include <arch/board/up_adc.h>
|
#include <arch/board/up_adc.h>
|
||||||
#include <arch/board/board.h>
|
#include <arch/board/board.h>
|
||||||
#include <arch/board/drv_led.h>
|
#include <arch/board/drv_led.h>
|
||||||
@@ -71,6 +70,8 @@
|
|||||||
|
|
||||||
#include <drivers/drv_hrt.h>
|
#include <drivers/drv_hrt.h>
|
||||||
|
|
||||||
|
#include <systemlib/cpuload.h>
|
||||||
|
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
* Pre-Processor Definitions
|
* Pre-Processor Definitions
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|||||||
@@ -41,14 +41,15 @@
|
|||||||
* channel.
|
* channel.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef _DRV_PWM_OUTPUT_H
|
#pragma once
|
||||||
#define _DRV_PWM_OUTPUT_H
|
|
||||||
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <sys/ioctl.h>
|
#include <sys/ioctl.h>
|
||||||
|
|
||||||
#include "drv_orb_dev.h"
|
#include "drv_orb_dev.h"
|
||||||
|
|
||||||
|
__BEGIN_DECLS
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Path for the default PWM output device.
|
* Path for the default PWM output device.
|
||||||
*
|
*
|
||||||
@@ -109,4 +110,63 @@ ORB_DECLARE(output_pwm);
|
|||||||
#define PWM_SERVO_GET(_servo) _IOC(_PWM_SERVO_BASE, 0x40 + _servo)
|
#define PWM_SERVO_GET(_servo) _IOC(_PWM_SERVO_BASE, 0x40 + _servo)
|
||||||
|
|
||||||
|
|
||||||
#endif /* _DRV_PWM_OUTPUT_H */
|
/*
|
||||||
|
* Low-level PWM output interface.
|
||||||
|
*
|
||||||
|
* This is the low-level API to the platform-specific PWM driver.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Intialise the PWM servo outputs using the specified configuration.
|
||||||
|
*
|
||||||
|
* @param channel_mask Bitmask of channels (LSB = channel 0) to enable.
|
||||||
|
* This allows some of the channels to remain configured
|
||||||
|
* as GPIOs or as another function.
|
||||||
|
* @return OK on success.
|
||||||
|
*/
|
||||||
|
__EXPORT extern int up_pwm_servo_init(uint32_t channel_mask);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* De-initialise the PWM servo outputs.
|
||||||
|
*/
|
||||||
|
__EXPORT extern void up_pwm_servo_deinit(void);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Arm or disarm servo outputs.
|
||||||
|
*
|
||||||
|
* When disarmed, servos output no pulse.
|
||||||
|
*
|
||||||
|
* @bug This function should, but does not, guarantee that any pulse
|
||||||
|
* currently in progress is cleanly completed.
|
||||||
|
*
|
||||||
|
* @param armed If true, outputs are armed; if false they
|
||||||
|
* are disarmed.
|
||||||
|
*/
|
||||||
|
__EXPORT extern void up_pwm_servo_arm(bool armed);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the servo update rate
|
||||||
|
*
|
||||||
|
* @param rate The update rate in Hz to set.
|
||||||
|
* @return OK on success, -ERANGE if an unsupported update rate is set.
|
||||||
|
*/
|
||||||
|
__EXPORT extern int up_pwm_servo_set_rate(unsigned rate);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the current output value for a channel.
|
||||||
|
*
|
||||||
|
* @param channel The channel to set.
|
||||||
|
* @param value The output pulse width in microseconds.
|
||||||
|
*/
|
||||||
|
__EXPORT extern int up_pwm_servo_set(unsigned channel, servo_position_t value);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Get the current output value for a channel.
|
||||||
|
*
|
||||||
|
* @param channel The channel to read.
|
||||||
|
* @return The output pulse width in microseconds, or zero if
|
||||||
|
* outputs are not armed or not configured.
|
||||||
|
*/
|
||||||
|
__EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel);
|
||||||
|
|
||||||
|
__END_DECLS
|
||||||
|
|||||||
@@ -0,0 +1,42 @@
|
|||||||
|
############################################################################
|
||||||
|
#
|
||||||
|
# Copyright (C) 2012 PX4 Development Team. 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. 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 PX4 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 OWNER 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.
|
||||||
|
#
|
||||||
|
############################################################################
|
||||||
|
|
||||||
|
#
|
||||||
|
# STM32 driver support code
|
||||||
|
#
|
||||||
|
# Modules in this directory are compiled for all STM32 targets.
|
||||||
|
#
|
||||||
|
|
||||||
|
INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
|
||||||
|
|
||||||
|
include $(APPDIR)/mk/app.mk
|
||||||
@@ -32,6 +32,8 @@
|
|||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
* @file drv_pwm_servo.c
|
||||||
|
*
|
||||||
* Servo driver supporting PWM servos connected to STM32 timer blocks.
|
* Servo driver supporting PWM servos connected to STM32 timer blocks.
|
||||||
*
|
*
|
||||||
* Works with any of the 'generic' or 'advanced' STM32 timers that
|
* Works with any of the 'generic' or 'advanced' STM32 timers that
|
||||||
@@ -54,7 +56,7 @@
|
|||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
|
||||||
#include <arch/board/board.h>
|
#include <arch/board/board.h>
|
||||||
#include <arch/board/up_pwm_servo.h>
|
#include <drivers/drv_pwm_output.h>
|
||||||
|
|
||||||
#include "chip.h"
|
#include "chip.h"
|
||||||
#include "up_internal.h"
|
#include "up_internal.h"
|
||||||
@@ -46,7 +46,7 @@
|
|||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include <poll.h>
|
#include <poll.h>
|
||||||
|
|
||||||
#include <arch/board/up_cpuload.h>
|
#include <systemlib/cpuload.h>
|
||||||
#include <drivers/drv_hrt.h>
|
#include <drivers/drv_hrt.h>
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -1,62 +0,0 @@
|
|||||||
/****************************************************************************
|
|
||||||
*
|
|
||||||
* Copyright (C) 2012 PX4 Development Team. 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. 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 PX4 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 OWNER 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.
|
|
||||||
*
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
#ifndef UP_CPULOAD_H_
|
|
||||||
#define UP_CPULOAD_H_
|
|
||||||
|
|
||||||
#ifdef CONFIG_SCHED_INSTRUMENTATION
|
|
||||||
|
|
||||||
#include <nuttx/sched.h>
|
|
||||||
|
|
||||||
struct system_load_taskinfo_s {
|
|
||||||
uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
|
|
||||||
uint64_t curr_start_time; ///< Start time of the current scheduling slot
|
|
||||||
uint64_t start_time; ///< FIRST start time of task
|
|
||||||
FAR struct _TCB *tcb; ///<
|
|
||||||
bool valid; ///< Task is currently active / valid
|
|
||||||
};
|
|
||||||
|
|
||||||
struct system_load_s {
|
|
||||||
uint64_t start_time; ///< Global start time of measurements
|
|
||||||
struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS];
|
|
||||||
uint8_t initialized;
|
|
||||||
int total_count;
|
|
||||||
int running_count;
|
|
||||||
int sleeping_count;
|
|
||||||
};
|
|
||||||
|
|
||||||
void cpuload_initialize_once(void);
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#endif
|
|
||||||
@@ -95,6 +95,7 @@ CONFIGURED_APPS += drivers/mpu6000
|
|||||||
CONFIGURED_APPS += drivers/bma180
|
CONFIGURED_APPS += drivers/bma180
|
||||||
CONFIGURED_APPS += drivers/l3gd20
|
CONFIGURED_APPS += drivers/l3gd20
|
||||||
CONFIGURED_APPS += drivers/px4io
|
CONFIGURED_APPS += drivers/px4io
|
||||||
|
CONFIGURED_APPS += drivers/stm32
|
||||||
CONFIGURED_APPS += drivers/stm32/tone_alarm
|
CONFIGURED_APPS += drivers/stm32/tone_alarm
|
||||||
CONFIGURED_APPS += px4/fmu
|
CONFIGURED_APPS += px4/fmu
|
||||||
|
|
||||||
|
|||||||
@@ -43,7 +43,7 @@ AOBJS = $(ASRCS:.S=$(OBJEXT))
|
|||||||
CSRCS = up_boot.c up_leds.c up_spi.c \
|
CSRCS = up_boot.c up_leds.c up_spi.c \
|
||||||
drv_gpio.c \
|
drv_gpio.c \
|
||||||
drv_led.c drv_eeprom.c \
|
drv_led.c drv_eeprom.c \
|
||||||
up_pwm_servo.c up_usbdev.c
|
up_usbdev.c
|
||||||
|
|
||||||
ifeq ($(CONFIG_ADC),y)
|
ifeq ($(CONFIG_ADC),y)
|
||||||
CSRCS += up_adc.c
|
CSRCS += up_adc.c
|
||||||
|
|||||||
@@ -1,183 +0,0 @@
|
|||||||
/****************************************************************************
|
|
||||||
* configs/px4fmu/src/up_leds.c
|
|
||||||
* arch/arm/src/board/up_leds.c
|
|
||||||
*
|
|
||||||
* Copyright (C) 2011 Gregory Nutt. All rights reserved.
|
|
||||||
* Author: Gregory Nutt <gnutt@nuttx.org>
|
|
||||||
*
|
|
||||||
* 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 NuttX 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 OWNER 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.
|
|
||||||
*
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
/****************************************************************************
|
|
||||||
* Included Files
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
#include <nuttx/config.h>
|
|
||||||
|
|
||||||
#include <stdint.h>
|
|
||||||
#include <stdbool.h>
|
|
||||||
#include <debug.h>
|
|
||||||
|
|
||||||
#include <sys/time.h>
|
|
||||||
#include <sched.h>
|
|
||||||
|
|
||||||
#include <arch/board/board.h>
|
|
||||||
#include <arch/board/up_hrt.h>
|
|
||||||
#include <arch/board/up_cpuload.h>
|
|
||||||
|
|
||||||
#include "chip.h"
|
|
||||||
#include "up_arch.h"
|
|
||||||
#include "up_internal.h"
|
|
||||||
#include "stm32_internal.h"
|
|
||||||
#include "px4fmu-internal.h"
|
|
||||||
|
|
||||||
/****************************************************************************
|
|
||||||
* Definitions
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
|
|
||||||
/****************************************************************************
|
|
||||||
* Public Functions
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
/****************************************************************************
|
|
||||||
* Name:
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
#ifdef CONFIG_SCHED_INSTRUMENTATION
|
|
||||||
|
|
||||||
struct system_load_s system_load;
|
|
||||||
|
|
||||||
extern FAR _TCB *sched_gettcb(pid_t pid);
|
|
||||||
|
|
||||||
void cpuload_initialize_once(void);
|
|
||||||
|
|
||||||
void cpuload_initialize_once()
|
|
||||||
{
|
|
||||||
// if (!system_load.initialized)
|
|
||||||
// {
|
|
||||||
system_load.start_time = hrt_absolute_time();
|
|
||||||
int i;
|
|
||||||
for (i = 0; i < CONFIG_MAX_TASKS; i++)
|
|
||||||
{
|
|
||||||
system_load.tasks[i].valid = false;
|
|
||||||
}
|
|
||||||
system_load.total_count = 0;
|
|
||||||
|
|
||||||
uint64_t now = hrt_absolute_time();
|
|
||||||
|
|
||||||
/* initialize idle thread statically */
|
|
||||||
system_load.tasks[0].start_time = now;
|
|
||||||
system_load.tasks[0].total_runtime = 0;
|
|
||||||
system_load.tasks[0].curr_start_time = 0;
|
|
||||||
system_load.tasks[0].tcb = sched_gettcb(0);
|
|
||||||
system_load.tasks[0].valid = true;
|
|
||||||
system_load.total_count++;
|
|
||||||
|
|
||||||
/* initialize init thread statically */
|
|
||||||
system_load.tasks[1].start_time = now;
|
|
||||||
system_load.tasks[1].total_runtime = 0;
|
|
||||||
system_load.tasks[1].curr_start_time = 0;
|
|
||||||
system_load.tasks[1].tcb = sched_gettcb(1);
|
|
||||||
system_load.tasks[1].valid = true;
|
|
||||||
/* count init thread */
|
|
||||||
system_load.total_count++;
|
|
||||||
// }
|
|
||||||
}
|
|
||||||
|
|
||||||
void sched_note_start(FAR _TCB *tcb )
|
|
||||||
{
|
|
||||||
/* search first free slot */
|
|
||||||
int i;
|
|
||||||
for (i = 1; i < CONFIG_MAX_TASKS; i++)
|
|
||||||
{
|
|
||||||
if (!system_load.tasks[i].valid)
|
|
||||||
{
|
|
||||||
/* slot is available */
|
|
||||||
system_load.tasks[i].start_time = hrt_absolute_time();
|
|
||||||
system_load.tasks[i].total_runtime = 0;
|
|
||||||
system_load.tasks[i].curr_start_time = 0;
|
|
||||||
system_load.tasks[i].tcb = tcb;
|
|
||||||
system_load.tasks[i].valid = true;
|
|
||||||
system_load.total_count++;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void sched_note_stop(FAR _TCB *tcb )
|
|
||||||
{
|
|
||||||
int i;
|
|
||||||
for (i = 1; i < CONFIG_MAX_TASKS; i++)
|
|
||||||
{
|
|
||||||
if (system_load.tasks[i].tcb->pid == tcb->pid)
|
|
||||||
{
|
|
||||||
/* mark slot as fee */
|
|
||||||
system_load.tasks[i].valid = false;
|
|
||||||
system_load.tasks[i].total_runtime = 0;
|
|
||||||
system_load.tasks[i].curr_start_time = 0;
|
|
||||||
system_load.tasks[i].tcb = NULL;
|
|
||||||
system_load.total_count--;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb)
|
|
||||||
{
|
|
||||||
uint64_t new_time = hrt_absolute_time();
|
|
||||||
|
|
||||||
/* Kind of inefficient: find both tasks and update times */
|
|
||||||
uint8_t both_found = 0;
|
|
||||||
for (int i = 0; i < CONFIG_MAX_TASKS; i++)
|
|
||||||
{
|
|
||||||
/* Task ending its current scheduling run */
|
|
||||||
if (system_load.tasks[i].tcb->pid == pFromTcb->pid)
|
|
||||||
{
|
|
||||||
//if (system_load.tasks[i].curr_start_time != 0)
|
|
||||||
{
|
|
||||||
system_load.tasks[i].total_runtime += new_time - system_load.tasks[i].curr_start_time;
|
|
||||||
}
|
|
||||||
both_found++;
|
|
||||||
}
|
|
||||||
else if (system_load.tasks[i].tcb->pid == pToTcb->pid)
|
|
||||||
{
|
|
||||||
system_load.tasks[i].curr_start_time = new_time;
|
|
||||||
both_found++;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Do only iterate as long as needed */
|
|
||||||
if (both_found == 2)
|
|
||||||
{
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif /* CONFIG_SCHED_INSTRUMENTATION */
|
|
||||||
@@ -1,280 +0,0 @@
|
|||||||
/****************************************************************************
|
|
||||||
* config/px4fmu/src/up_nsh.c
|
|
||||||
* arch/arm/src/board/up_nsh.c
|
|
||||||
*
|
|
||||||
* Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
|
|
||||||
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
|
|
||||||
*
|
|
||||||
* 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 NuttX 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 OWNER 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.
|
|
||||||
*
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
/****************************************************************************
|
|
||||||
* Included Files
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
#include <nuttx/config.h>
|
|
||||||
|
|
||||||
#include <stdbool.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <debug.h>
|
|
||||||
#include <errno.h>
|
|
||||||
|
|
||||||
#include <nuttx/arch.h>
|
|
||||||
#include <nuttx/spi.h>
|
|
||||||
#include <nuttx/i2c.h>
|
|
||||||
#include <nuttx/mmcsd.h>
|
|
||||||
#include <nuttx/analog/adc.h>
|
|
||||||
#include <nuttx/arch.h>
|
|
||||||
|
|
||||||
#include "stm32_internal.h"
|
|
||||||
#include "px4fmu-internal.h"
|
|
||||||
#include "stm32_uart.h"
|
|
||||||
|
|
||||||
#include <arch/board/up_hrt.h>
|
|
||||||
#include <arch/board/up_cpuload.h>
|
|
||||||
#include <arch/board/up_adc.h>
|
|
||||||
#include <arch/board/board.h>
|
|
||||||
#include <arch/board/drv_led.h>
|
|
||||||
#include <arch/board/drv_eeprom.h>
|
|
||||||
|
|
||||||
/****************************************************************************
|
|
||||||
* Pre-Processor Definitions
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
/* Configuration ************************************************************/
|
|
||||||
|
|
||||||
/* Debug ********************************************************************/
|
|
||||||
|
|
||||||
#ifdef CONFIG_CPP_HAVE_VARARGS
|
|
||||||
# ifdef CONFIG_DEBUG
|
|
||||||
# define message(...) lib_lowprintf(__VA_ARGS__)
|
|
||||||
# else
|
|
||||||
# define message(...) printf(__VA_ARGS__)
|
|
||||||
# endif
|
|
||||||
#else
|
|
||||||
# ifdef CONFIG_DEBUG
|
|
||||||
# define message lib_lowprintf
|
|
||||||
# else
|
|
||||||
# define message printf
|
|
||||||
# endif
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/****************************************************************************
|
|
||||||
* Protected Functions
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
/****************************************************************************
|
|
||||||
* Public Functions
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
/****************************************************************************
|
|
||||||
* Name: nsh_archinitialize
|
|
||||||
*
|
|
||||||
* Description:
|
|
||||||
* Perform architecture specific initialization
|
|
||||||
*
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
static struct spi_dev_s *spi1;
|
|
||||||
static struct spi_dev_s *spi3;
|
|
||||||
static struct i2c_dev_s *i2c1;
|
|
||||||
static struct i2c_dev_s *i2c2;
|
|
||||||
static struct i2c_dev_s *i2c3;
|
|
||||||
|
|
||||||
#include <math.h>
|
|
||||||
|
|
||||||
#ifdef __cplusplus
|
|
||||||
int matherr(struct __exception *e) {
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
int matherr(struct exception *e) {
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
int nsh_archinitialize(void)
|
|
||||||
{
|
|
||||||
int result;
|
|
||||||
|
|
||||||
/* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */
|
|
||||||
|
|
||||||
/* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */
|
|
||||||
|
|
||||||
/* configure the high-resolution time/callout interface */
|
|
||||||
#ifdef CONFIG_HRT_TIMER
|
|
||||||
hrt_init();
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/* configure CPU load estimation */
|
|
||||||
#ifdef CONFIG_SCHED_INSTRUMENTATION
|
|
||||||
cpuload_initialize_once();
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/* set up the serial DMA polling */
|
|
||||||
#ifdef SERIAL_HAVE_DMA
|
|
||||||
{
|
|
||||||
static struct hrt_call serial_dma_call;
|
|
||||||
struct timespec ts;
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Poll at 1ms intervals for received bytes that have not triggered
|
|
||||||
* a DMA event.
|
|
||||||
*/
|
|
||||||
ts.tv_sec = 0;
|
|
||||||
ts.tv_nsec = 1000000;
|
|
||||||
|
|
||||||
hrt_call_every(&serial_dma_call,
|
|
||||||
ts_to_abstime(&ts),
|
|
||||||
ts_to_abstime(&ts),
|
|
||||||
(hrt_callout)stm32_serial_dma_poll,
|
|
||||||
NULL);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
message("\r\n");
|
|
||||||
|
|
||||||
up_ledoff(LED_BLUE);
|
|
||||||
up_ledoff(LED_AMBER);
|
|
||||||
|
|
||||||
up_ledon(LED_BLUE);
|
|
||||||
|
|
||||||
/* Configure user-space led driver */
|
|
||||||
px4fmu_led_init();
|
|
||||||
|
|
||||||
/* Configure SPI-based devices */
|
|
||||||
|
|
||||||
spi1 = up_spiinitialize(1);
|
|
||||||
if (!spi1)
|
|
||||||
{
|
|
||||||
message("[boot] FAILED to initialize SPI port 1\r\n");
|
|
||||||
up_ledon(LED_AMBER);
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Default SPI1 to 1MHz and de-assert the known chip selects.
|
|
||||||
SPI_SETFREQUENCY(spi1, 10000000);
|
|
||||||
SPI_SETBITS(spi1, 8);
|
|
||||||
SPI_SETMODE(spi1, SPIDEV_MODE3);
|
|
||||||
SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
|
|
||||||
SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false);
|
|
||||||
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
|
|
||||||
up_udelay(20);
|
|
||||||
|
|
||||||
message("[boot] Successfully initialized SPI port 1\r\n");
|
|
||||||
|
|
||||||
/* initialize I2C2 bus */
|
|
||||||
|
|
||||||
i2c2 = up_i2cinitialize(2);
|
|
||||||
if (!i2c2) {
|
|
||||||
message("[boot] FAILED to initialize I2C bus 2\n");
|
|
||||||
up_ledon(LED_AMBER);
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* set I2C2 speed */
|
|
||||||
I2C_SETFREQUENCY(i2c2, 400000);
|
|
||||||
|
|
||||||
|
|
||||||
i2c3 = up_i2cinitialize(3);
|
|
||||||
if (!i2c3) {
|
|
||||||
message("[boot] FAILED to initialize I2C bus 3\n");
|
|
||||||
up_ledon(LED_AMBER);
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* set I2C3 speed */
|
|
||||||
I2C_SETFREQUENCY(i2c3, 400000);
|
|
||||||
|
|
||||||
/* try to attach, don't fail if device is not responding */
|
|
||||||
(void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS,
|
|
||||||
FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES,
|
|
||||||
FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES,
|
|
||||||
FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1);
|
|
||||||
|
|
||||||
#if defined(CONFIG_STM32_SPI3)
|
|
||||||
/* Get the SPI port */
|
|
||||||
|
|
||||||
message("[boot] Initializing SPI port 3\n");
|
|
||||||
spi3 = up_spiinitialize(3);
|
|
||||||
if (!spi3)
|
|
||||||
{
|
|
||||||
message("[boot] FAILED to initialize SPI port 3\n");
|
|
||||||
up_ledon(LED_AMBER);
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
message("[boot] Successfully initialized SPI port 3\n");
|
|
||||||
|
|
||||||
/* Now bind the SPI interface to the MMCSD driver */
|
|
||||||
result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
|
|
||||||
if (result != OK)
|
|
||||||
{
|
|
||||||
message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
|
|
||||||
up_ledon(LED_AMBER);
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
|
|
||||||
#endif /* SPI3 */
|
|
||||||
|
|
||||||
/* initialize I2C1 bus */
|
|
||||||
|
|
||||||
i2c1 = up_i2cinitialize(1);
|
|
||||||
if (!i2c1) {
|
|
||||||
message("[boot] FAILED to initialize I2C bus 1\n");
|
|
||||||
up_ledon(LED_AMBER);
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* set I2C1 speed */
|
|
||||||
I2C_SETFREQUENCY(i2c1, 400000);
|
|
||||||
|
|
||||||
/* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */
|
|
||||||
|
|
||||||
/* Get board information if available */
|
|
||||||
|
|
||||||
/* Initialize the user GPIOs */
|
|
||||||
px4fmu_gpio_init();
|
|
||||||
|
|
||||||
#ifdef CONFIG_ADC
|
|
||||||
int adc_state = adc_devinit();
|
|
||||||
if (adc_state != OK)
|
|
||||||
{
|
|
||||||
/* Try again */
|
|
||||||
adc_state = adc_devinit();
|
|
||||||
if (adc_state != OK)
|
|
||||||
{
|
|
||||||
/* Give up */
|
|
||||||
message("[boot] FAILED adc_devinit: %d\n", adc_state);
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
return OK;
|
|
||||||
}
|
|
||||||
Reference in New Issue
Block a user