fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:18:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
unknown
2013-07-09 17:10:49 -04:00
parent a9b327b1fe c3d07030dd
commit c264267814
7 changed files with 346 additions and 215 deletions
Download Patch File Download Diff File Expand all files Collapse all files
mavlink/include/mavlink/v1.0/mavlink_conversions.h
+6
View File
@@ -1,6 +1,12 @@
#ifndef _MAVLINK_CONVERSIONS_H_
#define _MAVLINK_CONVERSIONS_H_
/* enable math defines on Windows */
#ifdef _MSC_VER
#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif
#endif
#include <math.h>
/**
src/drivers/gps/ubx.cpp
+35 -25
View File
@@ -176,17 +176,17 @@ UBX::configure(unsigned &baudrate)
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC,
1);
UBX_CFG_MSG_PAYLOAD_RATE1_1HZ);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL,
1);
UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED,
1);
UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
@@ -196,7 +196,7 @@ UBX::configure(unsigned &baudrate)
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO,
0);
UBX_CFG_MSG_PAYLOAD_RATE1_05HZ);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
@@ -224,35 +224,18 @@ UBX::receive(unsigned timeout)
fds[0].fd = _fd;
fds[0].events = POLLIN;
uint8_t buf[32];
uint8_t buf[128];
/* timeout additional to poll */
uint64_t time_started = hrt_absolute_time();
int j = 0;
ssize_t count = 0;
bool position_updated = false;
while (true) {
/* pass received bytes to the packet decoder */
while (j < count) {
if (parse_char(buf[j]) > 0) {
/* return to configure during configuration or to the gps driver during normal work
* if a packet has arrived */
if (handle_message() > 0)
return 1;
}
/* in case we keep trying but only get crap from GPS */
if (time_started + timeout*1000 < hrt_absolute_time() ) {
return -1;
}
j++;
}
/* everything is read */
j = count = 0;
/* then poll for new data */
/* poll for new data */
int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
if (ret < 0) {
@@ -272,8 +255,26 @@ UBX::receive(unsigned timeout)
* available, we'll go back to poll() again...
*/
count = ::read(_fd, buf, sizeof(buf));
/* pass received bytes to the packet decoder */
for (int i = 0; i < count; i++) {
if (parse_char(buf[i])) {
/* return to configure during configuration or to the gps driver during normal work
* if a packet has arrived */
if (handle_message())
position_updated = true;
}
}
}
}
/* return success after receiving a packet */
if (position_updated)
return 1;
/* abort after timeout if no packet parsed successfully */
if (time_started + timeout*1000 < hrt_absolute_time() ) {
return -1;
}
}
}
@@ -327,6 +328,7 @@ UBX::parse_char(uint8_t b)
}
break;
case UBX_DECODE_GOT_CLASS:
{
add_byte_to_checksum(b);
switch (_message_class) {
case NAV:
@@ -413,6 +415,14 @@ UBX::parse_char(uint8_t b)
// config_needed = true;
break;
}
// Evaluate state machine - if the state changed,
// the state machine was reset via decode_init()
// and we want to tell the module to stop sending this message
// disable unknown message
//warnx("disabled class %d, msg %d", (int)_message_class, (int)b);
//configure_message_rate(_message_class, b, 0);
}
break;
case UBX_DECODE_GOT_MESSAGEID:
add_byte_to_checksum(b);
src/drivers/mpu6000/mpu6000.cpp
+24 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* 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
@@ -272,6 +272,11 @@ private:
*/
void _set_dlpf_filter(uint16_t frequency_hz);
/*
set sample rate (approximate) - 1kHz to 5Hz
*/
void _set_sample_rate(uint16_t desired_sample_rate_hz);
};
/**
@@ -378,7 +383,8 @@ MPU6000::init()
up_udelay(1000);
// SAMPLE RATE
write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
//write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
_set_sample_rate(200); // default sample rate = 200Hz
usleep(1000);
// FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter)
@@ -493,6 +499,18 @@ MPU6000::probe()
return -EIO;
}
/*
set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro
*/
void
MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz)
{
uint8_t div = 1000 / desired_sample_rate_hz;
if(div>200) div=200;
if(div<1) div=1;
write_reg(MPUREG_SMPLRT_DIV, div-1);
}
/*
set the DLPF filter frequency. This affects both accel and gyro.
*/
@@ -644,8 +662,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
case ACCELIOCSSAMPLERATE:
case ACCELIOCGSAMPLERATE:
/* XXX not implemented */
return -EINVAL;
_set_sample_rate(arg);
return OK;
case ACCELIOCSLOWPASS:
case ACCELIOCGLOWPASS:
@@ -702,8 +720,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
case GYROIOCSSAMPLERATE:
case GYROIOCGSAMPLERATE:
/* XXX not implemented */
return -EINVAL;
_set_sample_rate(arg);
return OK;
case GYROIOCSLOWPASS:
case GYROIOCGLOWPASS:
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+24 -24
View File
@@ -44,42 +44,42 @@
/* Extended Kalman Filter covariances */
/* gyro process noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f);
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f);
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
/* gyro offsets process noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f);
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f);
/* gyro measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f);
PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f);
PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R0, 0.0008f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R1, 10000.0f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R2, 1.0f);
/* accelerometer measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R3, 0.0f);
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f);
PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f);
PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f);
int parameters_init(struct attitude_estimator_ekf_param_handles *h)
{
/* PID parameters */
h->q0 = param_find("EKF_ATT_V2_Q0");
h->q1 = param_find("EKF_ATT_V2_Q1");
h->q2 = param_find("EKF_ATT_V2_Q2");
h->q3 = param_find("EKF_ATT_V2_Q3");
h->q4 = param_find("EKF_ATT_V2_Q4");
h->q0 = param_find("EKF_ATT_V3_Q0");
h->q1 = param_find("EKF_ATT_V3_Q1");
h->q2 = param_find("EKF_ATT_V3_Q2");
h->q3 = param_find("EKF_ATT_V3_Q3");
h->q4 = param_find("EKF_ATT_V3_Q4");
h->r0 = param_find("EKF_ATT_V2_R0");
h->r1 = param_find("EKF_ATT_V2_R1");
h->r2 = param_find("EKF_ATT_V2_R2");
h->r3 = param_find("EKF_ATT_V2_R3");
h->r0 = param_find("EKF_ATT_V3_R0");
h->r1 = param_find("EKF_ATT_V3_R1");
h->r2 = param_find("EKF_ATT_V3_R2");
h->r3 = param_find("EKF_ATT_V3_R3");
h->roll_off = param_find("ATT_ROLL_OFFS");
h->pitch_off = param_find("ATT_PITCH_OFFS");
h->yaw_off = param_find("ATT_YAW_OFFS");
h->roll_off = param_find("ATT_ROLL_OFF3");
h->pitch_off = param_find("ATT_PITCH_OFF3");
h->yaw_off = param_find("ATT_YAW_OFF3");
return OK;
}
src/modules/gpio_led/gpio_led.c
+89 -21
View File
@@ -54,9 +54,15 @@
#include <poll.h>
#include <drivers/drv_gpio.h>
#define PX4IO_RELAY1 (1<<0)
#define PX4IO_RELAY2 (1<<1)
#define PX4IO_ACC1 (1<<2)
#define PX4IO_ACC2 (1<<3)
struct gpio_led_s {
struct work_s work;
int gpio_fd;
bool use_io;
int pin;
struct vehicle_status_s status;
int vehicle_status_sub;
@@ -75,51 +81,97 @@ void gpio_led_cycle(FAR void *arg);
int gpio_led_main(int argc, char *argv[])
{
int pin = GPIO_EXT_1;
if (argc < 2) {
errx(1, "no argument provided. Try 'start' or 'stop' [-p 1/2]");
errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n"
"\t-p\tUse pin:\n"
"\t\t1\tPX4FMU GPIO_EXT1 (default)\n"
"\t\t2\tPX4FMU GPIO_EXT2\n"
"\t\ta1\tPX4IO ACC1\n"
"\t\ta2\tPX4IO ACC2\n"
"\t\tr1\tPX4IO RELAY1\n"
"\t\tr2\tPX4IO RELAY2");
} else {
/* START COMMAND HANDLING */
if (!strcmp(argv[1], "start")) {
if (gpio_led_started) {
errx(1, "already running");
}
bool use_io = false;
int pin = GPIO_EXT_1;
if (argc > 2) {
if (!strcmp(argv[1], "-p")) {
if (!strcmp(argv[2], "1")) {
if (!strcmp(argv[2], "-p")) {
if (!strcmp(argv[3], "1")) {
use_io = false;
pin = GPIO_EXT_1;
} else if (!strcmp(argv[2], "2")) {
} else if (!strcmp(argv[3], "2")) {
use_io = false;
pin = GPIO_EXT_2;
} else if (!strcmp(argv[3], "a1")) {
use_io = true;
pin = PX4IO_ACC1;
} else if (!strcmp(argv[3], "a2")) {
use_io = true;
pin = PX4IO_ACC2;
} else if (!strcmp(argv[3], "r1")) {
use_io = true;
pin = PX4IO_RELAY1;
} else if (!strcmp(argv[3], "r2")) {
use_io = true;
pin = PX4IO_RELAY2;
} else {
warnx("[gpio_led] Unsupported pin: %s\n", argv[2]);
exit(1);
errx(1, "unsupported pin: %s", argv[3]);
}
}
}
memset(&gpio_led_data, 0, sizeof(gpio_led_data));
gpio_led_data.use_io = use_io;
gpio_led_data.pin = pin;
int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0);
if (ret != 0) {
warnx("[gpio_led] Failed to queue work: %d\n", ret);
exit(1);
errx(1, "failed to queue work: %d", ret);
} else {
gpio_led_started = true;
char pin_name[24];
if (use_io) {
if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) {
sprintf(pin_name, "PX4IO ACC%i", (pin >> 3));
} else {
sprintf(pin_name, "PX4IO RELAY%i", pin);
}
} else {
sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin);
}
warnx("start, using pin: %s", pin_name);
}
exit(0);
/* STOP COMMAND HANDLING */
} else if (!strcmp(argv[1], "stop")) {
gpio_led_started = false;
if (gpio_led_started) {
gpio_led_started = false;
warnx("stop");
/* INVALID COMMAND */
} else {
errx(1, "not running");
}
} else {
errx(1, "unrecognized command '%s', only supporting 'start' or 'stop'", argv[1]);
@@ -131,11 +183,22 @@ void gpio_led_start(FAR void *arg)
{
FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
char *gpio_dev;
if (priv->use_io) {
gpio_dev = "/dev/px4io";
} else {
gpio_dev = "/dev/px4fmu";
}
/* open GPIO device */
priv->gpio_fd = open(GPIO_DEVICE_PATH, 0);
priv->gpio_fd = open(gpio_dev, 0);
if (priv->gpio_fd < 0) {
warnx("[gpio_led] GPIO: open fail\n");
// TODO find way to print errors
//printf("gpio_led: GPIO device \"%s\" open fail\n", gpio_dev);
gpio_led_started = false;
return;
}
@@ -150,11 +213,11 @@ void gpio_led_start(FAR void *arg)
int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0);
if (ret != 0) {
warnx("[gpio_led] Failed to queue work: %d\n", ret);
// TODO find way to print errors
//printf("gpio_led: failed to queue work: %d\n", ret);
gpio_led_started = false;
return;
}
warnx("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin);
}
void gpio_led_cycle(FAR void *arg)
@@ -211,7 +274,12 @@ void gpio_led_cycle(FAR void *arg)
if (priv->counter > 5)
priv->counter = 0;
/* repeat cycle at 5 Hz*/
if (gpio_led_started)
/* repeat cycle at 5 Hz */
if (gpio_led_started) {
work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000));
} else {
/* switch off LED on stop */
ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
}
}
src/modules/systemlib/cpuload.c
+20 -20
View File
@@ -71,8 +71,6 @@ extern FAR struct _TCB *sched_gettcb(pid_t pid);
void cpuload_initialize_once()
{
// if (!system_load.initialized)
// {
system_load.start_time = hrt_absolute_time();
int i;
@@ -80,27 +78,29 @@ void cpuload_initialize_once()
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++;
int static_tasks_count = 2; // there are at least 2 threads that should be initialized statically - "idle" and "init"
/* 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++;
// }
#ifdef CONFIG_PAGING
static_tasks_count++; // include paging thread in initialization
#endif /* CONFIG_PAGING */
#if CONFIG_SCHED_WORKQUEUE
static_tasks_count++; // include high priority work0 thread in initialization
#endif /* CONFIG_SCHED_WORKQUEUE */
#if CONFIG_SCHED_LPWORK
static_tasks_count++; // include low priority work1 thread in initialization
#endif /* CONFIG_SCHED_WORKQUEUE */
// perform static initialization of "system" threads
for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++)
{
system_load.tasks[system_load.total_count].start_time = now;
system_load.tasks[system_load.total_count].total_runtime = 0;
system_load.tasks[system_load.total_count].curr_start_time = 0;
system_load.tasks[system_load.total_count].tcb = sched_gettcb(system_load.total_count); // it is assumed that these static threads have consecutive PIDs
system_load.tasks[system_load.total_count].valid = true;
}
}
void sched_note_start(FAR struct tcb_s *tcb)
src/systemcmds/top/top.c
+148 -119
View File
@@ -51,19 +51,46 @@
#include <systemlib/cpuload.h>
#include <drivers/drv_hrt.h>
#define CL "\033[K" // clear line
/**
* Start the top application.
*/
__EXPORT int top_main(int argc, char *argv[]);
__EXPORT int top_main(void);
extern struct system_load_s system_load;
bool top_sigusr1_rcvd = false;
int top_main(int argc, char *argv[])
static const char *
tstate_name(const tstate_t s)
{
int t;
switch (s) {
case TSTATE_TASK_INVALID: return "init";
case TSTATE_TASK_PENDING: return "PEND";
case TSTATE_TASK_READYTORUN: return "READY";
case TSTATE_TASK_RUNNING: return "RUN";
case TSTATE_TASK_INACTIVE: return "inact";
case TSTATE_WAIT_SEM: return "w:sem";
#ifndef CONFIG_DISABLE_SIGNALS
case TSTATE_WAIT_SIG: return "w:sig";
#endif
#ifndef CONFIG_DISABLE_MQUEUE
case TSTATE_WAIT_MQNOTEMPTY: return "w:mqe";
case TSTATE_WAIT_MQNOTFULL: return "w:mqf";
#endif
#ifdef CONFIG_PAGING
case TSTATE_WAIT_PAGEFILL: return "w:pgf";
#endif
default:
return "ERROR";
}
}
int
top_main(void)
{
uint64_t total_user_time = 0;
int running_count = 0;
@@ -75,7 +102,7 @@ int top_main(int argc, char *argv[])
uint64_t last_times[CONFIG_MAX_TASKS];
float curr_loads[CONFIG_MAX_TASKS];
for (t = 0; t < CONFIG_MAX_TASKS; t++)
for (int t = 0; t < CONFIG_MAX_TASKS; t++)
last_times[t] = 0;
float interval_time_ms_inv = 0.f;
@@ -83,16 +110,16 @@ int top_main(int argc, char *argv[])
/* Open console directly to grab CTRL-C signal */
int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
while (true)
// for (t = 0; t < 10; t++)
{
int i;
/* clear screen */
printf("\033[2J");
uint64_t curr_time_ms = (hrt_absolute_time() / 1000LLU);
unsigned int curr_time_s = curr_time_ms / 1000LLU;
for (;;) {
int i;
uint64_t curr_time_us;
uint64_t idle_time_us;
uint64_t idle_time_total_ms = (system_load.tasks[0].total_runtime / 1000LLU);
unsigned int idle_time_total_s = idle_time_total_ms / 1000LLU;
curr_time_us = hrt_absolute_time();
idle_time_us = system_load.tasks[0].total_runtime;
if (new_time > interval_start_time)
interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000));
@@ -102,7 +129,38 @@ int top_main(int argc, char *argv[])
total_user_time = 0;
for (i = 0; i < CONFIG_MAX_TASKS; i++) {
uint64_t interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && system_load.tasks[i].total_runtime > last_times[i]) ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 : 0;
uint64_t interval_runtime;
if (system_load.tasks[i].valid) {
switch (system_load.tasks[i].tcb->task_state) {
case TSTATE_TASK_PENDING:
case TSTATE_TASK_READYTORUN:
case TSTATE_TASK_RUNNING:
running_count++;
break;
case TSTATE_TASK_INVALID:
case TSTATE_TASK_INACTIVE:
case TSTATE_WAIT_SEM:
#ifndef CONFIG_DISABLE_SIGNALS
case TSTATE_WAIT_SIG:
#endif
#ifndef CONFIG_DISABLE_MQUEUE
case TSTATE_WAIT_MQNOTEMPTY:
case TSTATE_WAIT_MQNOTFULL:
#endif
#ifdef CONFIG_PAGING
case TSTATE_WAIT_PAGEFILL:
#endif
blocked_count++;
break;
}
}
interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 &&
system_load.tasks[i].total_runtime > last_times[i])
? (system_load.tasks[i].total_runtime - last_times[i]) / 1000
: 0;
last_times[i] = system_load.tasks[i].total_runtime;
@@ -111,7 +169,6 @@ int top_main(int argc, char *argv[])
if (i > 0)
total_user_time += interval_runtime;
} else
curr_loads[i] = 0;
}
@@ -119,127 +176,99 @@ int top_main(int argc, char *argv[])
for (i = 0; i < CONFIG_MAX_TASKS; i++) {
if (system_load.tasks[i].valid && (new_time > interval_start_time)) {
if (system_load.tasks[i].tcb->pid == 0) {
float idle = curr_loads[0];
float task_load = (float)(total_user_time) * interval_time_ms_inv;
float idle;
float task_load;
float sched_load;
if (task_load > (1.f - idle)) task_load = (1.f - idle); /* this can happen if one tasks total runtime was not computed correctly by the scheduler instrumentation TODO */
idle = curr_loads[0];
task_load = (float)(total_user_time) * interval_time_ms_inv;
float sched_load = 1.f - idle - task_load;
/* this can happen if one tasks total runtime was not computed
correctly by the scheduler instrumentation TODO */
if (task_load > (1.f - idle))
task_load = (1.f - idle);
sched_load = 1.f - idle - task_load;
/* print system information */
printf("\033[H"); /* cursor home */
printf("\033[KProcesses: %d total, %d running, %d sleeping\n", system_load.total_count, running_count, blocked_count);
printf("\033[KCPU usage: %d.%02d%% tasks, %d.%02d%% sched, %d.%02d%% idle\n", (int)(task_load * 100), (int)((task_load * 10000.0f) - (int)(task_load * 100.0f) * 100), (int)(sched_load * 100), (int)((sched_load * 10000.0f) - (int)(sched_load * 100.0f) * 100), (int)(idle * 100), (int)((idle * 10000.0f) - ((int)(idle * 100)) * 100));
printf("\033[KUptime: %u.%03u s total, %d.%03d s idle\n\033[K\n", curr_time_s, (unsigned int)(curr_time_ms - curr_time_s * 1000LLU), idle_time_total_s, (int)(idle_time_total_ms - idle_time_total_s * 1000));
printf("\033[H"); /* move cursor home and clear screen */
printf(CL "Processes: %d total, %d running, %d sleeping\n",
system_load.total_count,
running_count,
blocked_count);
printf(CL "CPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n",
(double)(task_load * 100.f),
(double)(sched_load * 100.f),
(double)(idle * 100.f));
printf(CL "Uptime: %.3fs total, %.3fs idle\n\n",
(double)curr_time_us / 1000000.d,
(double)idle_time_us / 1000000.d);
/* 34 chars command name length (32 chars plus two spaces) */
char header_spaces[CONFIG_TASK_NAME_SIZE + 1];
memset(header_spaces, ' ', CONFIG_TASK_NAME_SIZE);
header_spaces[CONFIG_TASK_NAME_SIZE] = '\0';
/* header for task list */
printf(CL "%4s %*-s %8s %6s %11s %10s %-6s\n",
"PID",
CONFIG_TASK_NAME_SIZE, "COMMAND",
"CPU(ms)",
"CPU(%)",
"USED/STACK",
"PRIO(BASE)",
#if CONFIG_RR_INTERVAL > 0
printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces);
"TSLICE"
#else
printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\n", header_spaces);
#endif
} else {
enum tstate_e task_state = (enum tstate_e)system_load.tasks[i].tcb->task_state;
if (task_state == TSTATE_TASK_PENDING ||
task_state == TSTATE_TASK_READYTORUN ||
task_state == TSTATE_TASK_RUNNING) {
running_count++;
}
if (task_state == TSTATE_TASK_INACTIVE || /* BLOCKED - Initialized but not yet activated */
task_state == TSTATE_WAIT_SEM /* BLOCKED - Waiting for a semaphore */
#ifndef CONFIG_DISABLE_SIGNALS
|| task_state == TSTATE_WAIT_SIG /* BLOCKED - Waiting for a signal */
#endif
#ifndef CONFIG_DISABLE_MQUEUE
|| task_state == TSTATE_WAIT_MQNOTEMPTY /* BLOCKED - Waiting for a MQ to become not empty. */
|| task_state == TSTATE_WAIT_MQNOTFULL /* BLOCKED - Waiting for a MQ to become not full. */
#endif
#ifdef CONFIG_PAGING
|| task_state == TSTATE_WAIT_PAGEFILL /* BLOCKED - Waiting for page fill */
#endif
) {
blocked_count++;
}
char spaces[CONFIG_TASK_NAME_SIZE + 2];
/* count name len */
int namelen = 0;
while (namelen < CONFIG_TASK_NAME_SIZE) {
if (system_load.tasks[i].tcb->name[namelen] == '\0') break;
namelen++;
}
int s = 0;
for (s = 0; s < CONFIG_TASK_NAME_SIZE + 2 - namelen; s++) {
spaces[s] = ' ';
}
spaces[s] = '\0';
char *runtime_spaces = " ";
if ((system_load.tasks[i].total_runtime / 1000) < 99) {
runtime_spaces = "";
}
unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
(uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
unsigned stack_free = 0;
uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
while (stack_free < stack_size) {
if (*stack_sweeper++ != 0xff)
break;
stack_free++;
}
printf("\033[K % 2d\t%s%s % 8lld ms%s \t % 2d.%03d \t % 4u / % 4u",
(int)system_load.tasks[i].tcb->pid,
system_load.tasks[i].tcb->name,
spaces,
(system_load.tasks[i].total_runtime / 1000),
runtime_spaces,
(int)(curr_loads[i] * 100),
(int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100),
stack_size - stack_free,
stack_size);
/* Print scheduling info with RR time slice */
#if CONFIG_RR_INTERVAL > 0
printf("\t%d\t(%d)\t\t%d\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority, (int)system_load.tasks[i].tcb->timeslice);
#else
/* Print scheduling info without time slice*/
printf("\t%d (%d)\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority);
"STATE"
#endif
);
}
unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
(uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
unsigned stack_free = 0;
uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
while (stack_free < stack_size) {
if (*stack_sweeper++ != 0xff)
break;
stack_free++;
}
printf(CL "%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ",
system_load.tasks[i].tcb->pid,
CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name,
(system_load.tasks[i].total_runtime / 1000),
(int)(curr_loads[i] * 100),
(int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100),
stack_size - stack_free,
stack_size,
system_load.tasks[i].tcb->sched_priority,
system_load.tasks[i].tcb->base_priority);
#if CONFIG_RR_INTERVAL > 0
/* print scheduling info with RR time slice */
printf(" %6d\n", system_load.tasks[i].tcb->timeslice);
#else
// print task state instead
printf(" %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state));
#endif
}
}
printf("\033[K[ Hit Ctrl-C to quit. ]\n\033[J");
fflush(stdout);
interval_start_time = new_time;
char c;
/* Sleep 200 ms waiting for user input four times */
/* Sleep 200 ms waiting for user input five times ~ 1s */
/* XXX use poll ... */
for (int k = 0; k < 4; k++) {
for (int k = 0; k < 5; k++) {
char c;
if (read(console, &c, 1) == 1) {
if (c == 0x03 || c == 0x63) {
printf("Abort\n");
switch (c) {
case 0x03: // ctrl-c
case 0x1b: // esc
case 'c':
case 'q':
close(console);
return OK;
/* not reached */
}
}