dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' of github.com:PX4/Firmware into fmuv2_bringup

This commit is contained in:
Lorenz Meier 2013-07-10 11:58:48 +02:00
parent e2458677c9 71ca3ec449
commit f27491d2e5
28 changed files with 648 additions and 300 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
mavlink/include/mavlink/v1.0/mavlink_conversions.h
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>
/**

101
src/drivers/gps/ubx.cpp
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);
@ -539,7 +549,7 @@ UBX::handle_message()
}
case NAV_SVINFO: {
// printf("GOT NAV_SVINFO MESSAGE\n");
// printf("GOT NAV_SVINFO MESSAGE\n");
if (!_waiting_for_ack) {
//this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message
@ -560,40 +570,27 @@ UBX::handle_message()
uint8_t satellites_used = 0;
int i;
// printf("Number of Channels: %d\n", packet_part1->numCh);
for (i = 0; i < packet_part1->numCh; i++) { //for each channel
/* Get satellite information from the buffer */
memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2);
packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2;
/* Write satellite information to global storage */
uint8_t sv_used = packet_part2->flags & 0x01;
/* Write satellite information in the global storage */
_gps_position->satellite_prn[i] = packet_part2->svid;
//if satellite information is healthy store the data
uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield
if (!unhealthy) {
if ((packet_part2->flags) & 1) { //flags is a bitfield
_gps_position->satellite_used[i] = 1;
satellites_used++;
} else {
_gps_position->satellite_used[i] = 0;
}
_gps_position->satellite_snr[i] = packet_part2->cno;
_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
} else {
_gps_position->satellite_used[i] = 0;
_gps_position->satellite_snr[i] = 0;
_gps_position->satellite_elevation[i] = 0;
_gps_position->satellite_azimuth[i] = 0;
if ( sv_used ) {
// Count SVs used for NAV.
satellites_used++;
}
// Record info for all channels, whether or not the SV is used for NAV.
_gps_position->satellite_used[i] = sv_used;
_gps_position->satellite_snr[i] = packet_part2->cno;
_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
_gps_position->satellite_prn[i] = packet_part2->svid;
}
for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused

125
src/drivers/mkblctrl/mkblctrl.cpp
View File

@ -74,6 +74,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/esc_status.h>
#include <systemlib/err.h>
@ -87,7 +88,7 @@
#define MOTOR_STATE_PRESENT_MASK 0x80
#define MOTOR_STATE_ERROR_MASK 0x7F
#define MOTOR_SPINUP_COUNTER 2500
#define ESC_UORB_PUBLISH_DELAY 200
class MK : public device::I2C
{
@ -119,6 +120,7 @@ public:
int set_pwm_rate(unsigned rate);
int set_motor_count(unsigned count);
int set_motor_test(bool motortest);
int set_overrideSecurityChecks(bool overrideSecurityChecks);
int set_px4mode(int px4mode);
int set_frametype(int frametype);
unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput);
@ -136,11 +138,15 @@ private:
unsigned int _motor;
int _px4mode;
int _frametype;
orb_advert_t _t_outputs;
orb_advert_t _t_actuators_effective;
orb_advert_t _t_esc_status;
unsigned int _num_outputs;
bool _primary_pwm_device;
bool _motortest;
bool _overrideSecurityChecks;
volatile bool _task_should_exit;
bool _armed;
@ -214,6 +220,7 @@ struct MotorData_t {
unsigned int Version; // the version of the BL (0 = old)
unsigned int SetPoint; // written by attitude controller
unsigned int SetPointLowerBits; // for higher Resolution of new BLs
float SetPoint_PX4; // Values from PX4
unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present
unsigned int ReadMode; // select data to read
unsigned short RawPwmValue; // length of PWM pulse
@ -243,8 +250,10 @@ MK::MK(int bus) :
_t_armed(-1),
_t_outputs(0),
_t_actuators_effective(0),
_t_esc_status(0),
_num_outputs(0),
_motortest(false),
_overrideSecurityChecks(false),
_motor(-1),
_px4mode(MAPPING_MK),
_frametype(FRAME_PLUS),
@ -464,6 +473,13 @@ MK::set_motor_test(bool motortest)
return OK;
}
int
MK::set_overrideSecurityChecks(bool overrideSecurityChecks)
{
_overrideSecurityChecks = overrideSecurityChecks;
return OK;
}
short
MK::scaling(float val, float inMin, float inMax, float outMin, float outMax)
{
@ -506,8 +522,6 @@ MK::task_main()
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
/* advertise the effective control inputs */
actuator_controls_effective_s controls_effective;
memset(&controls_effective, 0, sizeof(controls_effective));
@ -515,6 +529,12 @@ MK::task_main()
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
&controls_effective);
/* advertise the blctrl status */
esc_status_s esc;
memset(&esc, 0, sizeof(esc));
_t_esc_status = orb_advertise(ORB_ID(esc_status), &esc);
pollfd fds[2];
fds[0].fd = _t_actuators;
@ -602,9 +622,11 @@ MK::task_main()
}
}
/* don't go under BLCTRL_MIN_VALUE */
if (outputs.output[i] < BLCTRL_MIN_VALUE) {
outputs.output[i] = BLCTRL_MIN_VALUE;
if(!_overrideSecurityChecks) {
/* don't go under BLCTRL_MIN_VALUE */
if (outputs.output[i] < BLCTRL_MIN_VALUE) {
outputs.output[i] = BLCTRL_MIN_VALUE;
}
}
/* output to BLCtrl's */
@ -612,7 +634,10 @@ MK::task_main()
mk_servo_test(i);
} else {
mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine
//mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine
// 11 Bit
Motor[i].SetPoint_PX4 = outputs.output[i];
mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 2047)); // scale the output to 0 - 2047 and sent to output routine
}
}
@ -635,8 +660,43 @@ MK::task_main()
}
/*
* Only update esc topic every half second.
*/
if (hrt_absolute_time() - esc.timestamp > 500000) {
esc.counter++;
esc.timestamp = hrt_absolute_time();
esc.esc_count = (uint8_t) _num_outputs;
esc.esc_connectiontype = ESC_CONNECTION_TYPE_I2C;
for (unsigned int i = 0; i < _num_outputs; i++) {
esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i;
esc.esc[i].esc_vendor = ESC_VENDOR_MIKROKOPTER;
esc.esc[i].esc_version = (uint16_t) Motor[i].Version;
esc.esc[i].esc_voltage = (uint16_t) 0;
esc.esc[i].esc_current = (uint16_t) Motor[i].Current;
esc.esc[i].esc_rpm = (uint16_t) 0;
esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4;
if (Motor[i].Version == 1) {
// BLCtrl 2.0 (11Bit)
esc.esc[i].esc_setpoint_raw = (uint16_t) (Motor[i].SetPoint<<3) | Motor[i].SetPointLowerBits;
} else {
// BLCtrl < 2.0 (8Bit)
esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint;
}
esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
esc.esc[i].esc_state = (uint16_t) Motor[i].State;
esc.esc[i].esc_errorcount = (uint16_t) 0;
}
orb_publish(ORB_ID(esc_status), _t_esc_status, &esc);
}
}
//::close(_t_esc_status);
::close(_t_actuators);
::close(_t_actuators_effective);
::close(_t_armed);
@ -715,17 +775,17 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
}
if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
_task_should_exit = true;
if(!_overrideSecurityChecks) {
if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
_task_should_exit = true;
}
}
}
return foundMotorCount;
}
int
MK::mk_servo_set(unsigned int chan, short val)
{
@ -738,17 +798,15 @@ MK::mk_servo_set(unsigned int chan, short val)
tmpVal = val;
if (tmpVal > 1024) {
tmpVal = 1024;
if (tmpVal > 2047) {
tmpVal = 2047;
} else if (tmpVal < 0) {
tmpVal = 0;
}
Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
//Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 4;
Motor[chan].SetPointLowerBits = 0;
Motor[chan].SetPoint = (uint8_t)(tmpVal>>3)&0xff;
Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal%8)&0x07;
if (_armed == false) {
Motor[chan].SetPoint = 0;
@ -1014,8 +1072,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
if (arg < 2150) {
Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg;
mk_servo_set_value(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 1024));
mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 2047));
} else {
ret = -EINVAL;
}
@ -1112,25 +1169,19 @@ ssize_t
MK::write(file *filp, const char *buffer, size_t len)
{
unsigned count = len / 2;
// loeschen uint16_t values[4];
uint16_t values[8];
// loeschen if (count > 4) {
// loeschen // we only have 4 PWM outputs on the FMU
// loeschen count = 4;
// loeschen }
if (count > _num_outputs) {
// we only have 8 I2C outputs in the driver
count = _num_outputs;
}
// allow for misaligned values
memcpy(values, buffer, count * 2);
for (uint8_t i = 0; i < count; i++) {
Motor[i].RawPwmValue = (unsigned short)values[i];
mk_servo_set_value(i, scaling(values[i], 1010, 2100, 0, 1024));
mk_servo_set(i, scaling(values[i], 1010, 2100, 0, 2047));
}
return count * 2;
@ -1282,7 +1333,7 @@ enum FrameType {
PortMode g_port_mode;
int
mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype)
mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
{
uint32_t gpio_bits;
int shouldStop = 0;
@ -1341,6 +1392,9 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
/* motortest if enabled */
g_mk->set_motor_test(motortest);
/* ovveride security checks if enabled */
g_mk->set_overrideSecurityChecks(overrideSecurityChecks);
/* count used motors */
do {
@ -1406,6 +1460,7 @@ mkblctrl_main(int argc, char *argv[])
int px4mode = MAPPING_PX4;
int frametype = FRAME_PLUS; // + plus is default
bool motortest = false;
bool overrideSecurityChecks = false;
bool showHelp = false;
bool newMode = false;
@ -1461,11 +1516,21 @@ mkblctrl_main(int argc, char *argv[])
showHelp = true;
}
/* look for the optional --override-security-checks parameter */
if (strcmp(argv[i], "--override-security-checks") == 0) {
overrideSecurityChecks = true;
newMode = true;
}
}
if (showHelp) {
fprintf(stderr, "mkblctrl: help:\n");
fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [-h / --help]\n");
fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [--override-security-checks] [-h / --help]\n\n");
fprintf(stderr, "\t -mkmode frame {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n");
fprintf(stderr, "\t -b i2c_bus_number \t\t Set the i2c bus where the ESCs are connected to (default 1).\n");
fprintf(stderr, "\t -t motortest \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n");
fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n");
exit(1);
}
@ -1483,7 +1548,7 @@ mkblctrl_main(int argc, char *argv[])
/* parameter set ? */
if (newMode) {
/* switch parameter */
return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype);
return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
}
/* test, etc. here g*/

30
src/drivers/mpu6000/mpu6000.cpp
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:

2
src/examples/fixedwing_control/main.c
View File

@ -512,7 +512,7 @@ usage(const char *reason)
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
* to task_spawn_cmd().
*/
int ex_fixedwing_control_main(int argc, char *argv[])
{

4
src/examples/flow_position_control/flow_position_control_main.c
View File

@ -101,7 +101,7 @@ usage(const char *reason)
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_spawn().
* to task_spawn_cmd().
*/
int flow_position_control_main(int argc, char *argv[])
{
@ -118,7 +118,7 @@ int flow_position_control_main(int argc, char *argv[])
}
thread_should_exit = false;
deamon_task = task_spawn("flow_position_control",
deamon_task = task_spawn_cmd("flow_position_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 6,
4096,

4
src/examples/flow_position_estimator/flow_position_estimator_main.c
View File

@ -90,7 +90,7 @@ static void usage(const char *reason)
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
* to task_spawn_cmd().
*/
int flow_position_estimator_main(int argc, char *argv[])
{
@ -107,7 +107,7 @@ int flow_position_estimator_main(int argc, char *argv[])
}
thread_should_exit = false;
daemon_task = task_spawn("flow_position_estimator",
daemon_task = task_spawn_cmd("flow_position_estimator",
SCHED_RR,
SCHED_PRIORITY_MAX - 5,
4096,

4
src/examples/flow_speed_control/flow_speed_control_main.c
View File

@ -99,7 +99,7 @@ usage(const char *reason)
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_spawn().
* to task_spawn_cmd().
*/
int flow_speed_control_main(int argc, char *argv[])
{
@ -116,7 +116,7 @@ int flow_speed_control_main(int argc, char *argv[])
}
thread_should_exit = false;
deamon_task = task_spawn("flow_speed_control",
deamon_task = task_spawn_cmd("flow_speed_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 6,
4096,

48
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
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;
}

110
src/modules/gpio_led/gpio_led.c
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);
}
}

7
src/modules/mavlink/mavlink.c
View File

@ -49,7 +49,6 @@
#include <mqueue.h>
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>
@ -471,7 +470,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
}
void
mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
{
write(uart, ch, (size_t)(sizeof(uint8_t) * length));
}
@ -479,7 +478,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
/*
* Internal function to give access to the channel status for each channel
*/
mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
extern mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
{
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
return &m_mavlink_status[channel];
@ -488,7 +487,7 @@ mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
/*
* Internal function to give access to the channel buffer for each channel
*/
mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
{
static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
return &m_mavlink_buffer[channel];

8
src/modules/mavlink/mavlink_bridge_header.h
View File

@ -73,9 +73,11 @@ extern mavlink_system_t mavlink_system;
* @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0
* @param ch Character to send
*/
extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length);
extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
#include <v1.0/common/mavlink.h>
#endif /* MAVLINK_BRIDGE_HEADER_H */

1
src/modules/mavlink/mavlink_parameters.c
View File

@ -40,7 +40,6 @@
*/
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include "mavlink_parameters.h"
#include <uORB/uORB.h>
#include "math.h" /* isinf / isnan checks */

2
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -49,7 +49,6 @@
#include <fcntl.h>
#include <mqueue.h>
#include <string.h>
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>
@ -503,7 +502,6 @@ handle_message(mavlink_message_t *msg)
} else {
orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
}
warnx("IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;

1
src/modules/mavlink/missionlib.c
View File

@ -48,7 +48,6 @@
#include <mqueue.h>
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>

2
src/modules/mavlink/missionlib.h
View File

@ -39,7 +39,7 @@
#pragma once
#include <v1.0/common/mavlink.h>
#include "mavlink_bridge_header.h"
//extern void mavlink_wpm_send_message(mavlink_message_t *msg);
//extern void mavlink_wpm_send_gcs_string(const char *string);

1
src/modules/mavlink/orb_listener.c
View File

@ -47,7 +47,6 @@
#include <fcntl.h>
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>

1
src/modules/mavlink/waypoints.c
View File

@ -45,6 +45,7 @@
#include <unistd.h>
#include <stdio.h>
#include "mavlink_bridge_header.h"
#include "missionlib.h"
#include "waypoints.h"
#include "util.h"

10
src/modules/mavlink/waypoints.h
View File

@ -47,11 +47,11 @@
#include <v1.0/mavlink_types.h>
#ifndef MAVLINK_SEND_UART_BYTES
#define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len)
#endif
extern mavlink_system_t mavlink_system;
#include <v1.0/common/mavlink.h>
// #ifndef MAVLINK_SEND_UART_BYTES
// #define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len)
// #endif
//extern mavlink_system_t mavlink_system;
#include "mavlink_bridge_header.h"
#include <stdbool.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>

1
src/modules/mavlink_onboard/mavlink.c
View File

@ -49,7 +49,6 @@
#include <mqueue.h>
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>

2
src/modules/mavlink_onboard/mavlink_bridge_header.h
View File

@ -78,4 +78,6 @@ extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int len
mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan);
#include <v1.0/common/mavlink.h>
#endif /* MAVLINK_BRIDGE_HEADER_H */

1
src/modules/mavlink_onboard/mavlink_receiver.c
View File

@ -50,7 +50,6 @@
#include <mqueue.h>
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>

34
src/modules/sdlog2/sdlog2.c
View File

@ -79,6 +79,7 @@
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/esc_status.h>
#include <systemlib/systemlib.h>
@ -614,7 +615,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
const ssize_t fdsc = 17;
const ssize_t fdsc = 18;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
@ -642,6 +643,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct rc_channels_s rc;
struct differential_pressure_s diff_pres;
struct airspeed_s airspeed;
struct esc_status_s esc;
} buf;
memset(&buf, 0, sizeof(buf));
@ -663,6 +665,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int flow_sub;
int rc_sub;
int airspeed_sub;
int esc_sub;
} subs;
/* log message buffer: header + body */
@ -686,6 +689,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_ARSP_s log_ARSP;
struct log_FLOW_s log_FLOW;
struct log_GPOS_s log_GPOS;
struct log_ESC_s log_ESC;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@ -795,6 +799,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ESCs --- */
subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
fds[fdsc_count].fd = subs.esc_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
@ -1105,6 +1115,28 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(AIRS);
}
/* --- ESCs --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc);
for (uint8_t i=0; i<buf.esc.esc_count; i++)
{
log_msg.msg_type = LOG_ESC_MSG;
log_msg.body.log_ESC.counter = buf.esc.counter;
log_msg.body.log_ESC.esc_count = buf.esc.esc_count;
log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype;
log_msg.body.log_ESC.esc_num = i;
log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address;
log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version;
log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage;
log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current;
log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm;
log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature;
log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint;
log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw;
LOGBUFFER_WRITE_AND_COUNT(ESC);
}
}
#ifdef SDLOG2_DEBUG
printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
#endif

19
src/modules/sdlog2/sdlog2_messages.h
View File

@ -209,6 +209,24 @@ struct log_GPOS_s {
float vel_e;
float vel_d;
};
/* --- ESC - ESC STATE --- */
#define LOG_ESC_MSG 64
struct log_ESC_s {
uint16_t counter;
uint8_t esc_count;
uint8_t esc_connectiontype;
uint8_t esc_num;
uint16_t esc_address;
uint16_t esc_version;
uint16_t esc_voltage;
uint16_t esc_current;
uint16_t esc_rpm;
uint16_t esc_temperature;
float esc_setpoint;
uint16_t esc_setpoint_raw;
};
#pragma pack(pop)
/* construct list of all message formats */
@ -230,6 +248,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
};
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);

40
src/modules/systemlib/cpuload.c
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)

3
src/modules/uORB/objects_common.cpp
View File

@ -169,3 +169,6 @@ ORB_DEFINE(debug_key_value, struct debug_key_value_s);
#include "topics/navigation_capabilities.h"
ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s);
#include "topics/esc_status.h"
ORB_DEFINE(esc_status, struct esc_status_s);

114
src/modules/uORB/topics/esc_status.h Normal file
View File

@ -0,0 +1,114 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: @author Marco Bauer <marco@wtns.de>
*
* 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.
*
****************************************************************************/
/**
* @file esc_status.h
* Definition of the esc_status uORB topic.
*
* Published the state machine and the system status bitfields
* (see SYS_STATUS mavlink message), published only by commander app.
*
* All apps should write to subsystem_info:
*
* (any app) --> subsystem_info (published) --> (commander app state machine) --> esc_status --> (mavlink app)
*/
#ifndef ESC_STATUS_H_
#define ESC_STATUS_H_
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics @{
*/
/**
* The number of ESCs supported.
* Current (Q2/2013) we support 8 ESCs,
*/
#define CONNECTED_ESC_MAX 8
enum ESC_VENDOR {
ESC_VENDOR_GENERIC = 0, /**< generic ESC */
ESC_VENDOR_MIKROKOPTER /**< Mikrokopter */
};
enum ESC_CONNECTION_TYPE {
ESC_CONNECTION_TYPE_PPM = 0, /**< Traditional PPM ESC */
ESC_CONNECTION_TYPE_SERIAL, /**< Serial Bus connected ESC */
ESC_CONNECTION_TYPE_ONESHOOT, /**< One Shoot PPM */
ESC_CONNECTION_TYPE_I2C, /**< I2C */
ESC_CONNECTION_TYPE_CAN /**< CAN-Bus */
};
/**
*
*/
struct esc_status_s
{
/* use of a counter and timestamp recommended (but not necessary) */
uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
uint8_t esc_count; /**< number of connected ESCs */
enum ESC_CONNECTION_TYPE esc_connectiontype; /**< how ESCs connected to the system */
struct {
uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */
enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */
uint16_t esc_version; /**< Version of current ESC - if supported */
uint16_t esc_voltage; /**< Voltage measured from current ESC - if supported */
uint16_t esc_current; /**< Current measured from current ESC (100mA steps) - if supported */
uint16_t esc_rpm; /**< RPM measured from current ESC - if supported */
uint16_t esc_temperature; /**< Temperature measured from current ESC - if supported */
float esc_setpoint; /**< setpoint of current ESC */
uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */
uint16_t esc_state; /**< State of ESC - depend on Vendor */
uint16_t esc_errorcount; /**< Number of reported errors by ESC - if supported */
} esc[CONNECTED_ESC_MAX];
};
/**
* @}
*/
/* register this as object request broker structure */
//ORB_DECLARE(esc_status);
ORB_DECLARE_OPTIONAL(esc_status);
#endif

267
src/systemcmds/top/top.c
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 */
}
}