diff options
28 files changed, 643 insertions, 295 deletions
diff --git a/mavlink/include/mavlink/v1.0/mavlink_conversions.h b/mavlink/include/mavlink/v1.0/mavlink_conversions.h index 1dc9088b7..d89363577 100644 --- a/mavlink/include/mavlink/v1.0/mavlink_conversions.h +++ b/mavlink/include/mavlink/v1.0/mavlink_conversions.h @@ -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> /** diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index f2e7ca67d..762c257aa 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -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; - 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++; - } + bool position_updated = false; - /* everything is read */ - j = count = 0; + while (true) { - /* 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 diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index b54b7aba1..e78d96569 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -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*/ diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index df1958186..220842536 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -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) @@ -494,6 +500,18 @@ MPU6000::probe() } /* + 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. */ void @@ -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: diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 27888523b..d13ffe414 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -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[]) { diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c index c177c8fd2..c96f73155 100644 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -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, diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index c0b16d2e7..e40c9081d 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -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, diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c index 9648728c8..8b3881c43 100644 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -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, diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 7d3812abd..52dac652b 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -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; } diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 43cbe74c7..8b4c0cb30 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -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); + } } diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 5b8345e7e..7c1c4b175 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -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]; diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 0010bb341..149efda60 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -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 */ diff --git a/src/modules/mavlink/mavlink_parameters.c b/src/modules/mavlink/mavlink_parameters.c index 819f3441b..18ca7a854 100644 --- a/src/modules/mavlink/mavlink_parameters.c +++ b/src/modules/mavlink/mavlink_parameters.c @@ -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 */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 33ac14860..01bbabd46 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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; diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index 4b010dd59..be88b8794 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -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> diff --git a/src/modules/mavlink/missionlib.h b/src/modules/mavlink/missionlib.h index c2ca735b3..c7d8f90c5 100644 --- a/src/modules/mavlink/missionlib.h +++ b/src/modules/mavlink/missionlib.h @@ -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); diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 0597555ab..edb8761b8 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -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> diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index cefcca468..405046750 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -45,6 +45,7 @@ #include <unistd.h> #include <stdio.h> +#include "mavlink_bridge_header.h" #include "missionlib.h" #include "waypoints.h" #include "util.h" diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index c32ab32e5..96a0ecd30 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -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> diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index cb6d6b16a..20fb11b2c 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -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> diff --git a/src/modules/mavlink_onboard/mavlink_bridge_header.h b/src/modules/mavlink_onboard/mavlink_bridge_header.h index 3ad3bb617..b72bbb2b1 100644 --- a/src/modules/mavlink_onboard/mavlink_bridge_header.h +++ b/src/modules/mavlink_onboard/mavlink_bridge_header.h @@ -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 */ diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c index 0acbea675..68d49c24b 100644 --- a/src/modules/mavlink_onboard/mavlink_receiver.c +++ b/src/modules/mavlink_onboard/mavlink_receiver.c @@ -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> diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index deac9e20b..3e6b20472 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -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 diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 4a66fe116..abc882d23 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -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); diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index 8fdff8ac0..afc5b072c 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -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++; - - /* 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++; - // } + int static_tasks_count = 2; // there are at least 2 threads that should be initialized statically - "idle" and "init" + +#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) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index e7d7e7bca..ae5fc6c61 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -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); diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h new file mode 100644 index 000000000..e67a39e1e --- /dev/null +++ b/src/modules/uORB/topics/esc_status.h @@ -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 diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index efe62685c..0d064a05e 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -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)); - - /* 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'; + 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); + + /* 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 */ + "STATE" #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; - 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; - while (stack_free < stack_size) { - if (*stack_sweeper++ != 0xff) - break; + stack_free++; + } - 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); - 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); + /* print scheduling info with RR time slice */ + printf(" %6d\n", 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); + // 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 */ } } |