diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index a4c1e832dc..3714b612fc 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -8,3 +8,5 @@ sh /etc/init.d/rc.fw_defaults set MIXER Viper + +set FAILSAFE "-c567 -p 1000" diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 1de0abb58d..e44cd0953d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -77,4 +77,9 @@ then pwm max -c $PWM_OUTPUTS -p $PWM_MAX fi fi + + if [ $FAILSAFE != none ] + then + pwm failsafe -d $OUTPUT_DEV $FAILSAFE + fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 1957719057..ea04ece34d 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -66,6 +66,9 @@ then # sercon + # Try to get an USB console + nshterm /dev/ttyACM0 & + # # Start the ORB (first app to start) # @@ -96,11 +99,9 @@ then # if rgbled start then - echo "[init] RGB Led" else if blinkm start then - echo "[init] BlinkM" blinkm systemstate fi fi @@ -129,6 +130,7 @@ then set LOAD_DEFAULT_APPS yes set GPS yes set GPS_FAKE no + set FAILSAFE none # # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts @@ -279,9 +281,6 @@ then fi fi - # Try to get an USB console - nshterm /dev/ttyACM0 & - # # Start the datamanager (and do not abort boot if it fails) # diff --git a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix index 0ec663e35e..7fed488af1 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix @@ -64,21 +64,22 @@ O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 -Gimbal / flaps / payload mixer for last four channels +Gimbal / flaps / payload mixer for last four channels, +using the payload control group ----------------------------------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 2 2 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 +S: 2 3 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/Viper.mix b/ROMFS/px4fmu_common/mixers/Viper.mix index 79c4447bee..5a0381bd87 100755 --- a/ROMFS/px4fmu_common/mixers/Viper.mix +++ b/ROMFS/px4fmu_common/mixers/Viper.mix @@ -52,21 +52,20 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 -Gimbal / flaps / payload mixer for last four channels +Inputs to the mixer come from channel group 2 (payload), channels 0 +(bay servo 1), 1 (bay servo 2) and 3 (drop release). ----------------------------------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 2 2 -10000 -10000 0 -10000 10000 + -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index d0a40445d6..5f146686c8 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -41,6 +41,7 @@ MODULES += drivers/frsky_telemetry MODULES += modules/sensors MODULES += drivers/mkblctrl MODULES += drivers/pca8574 +MODULES += drivers/px4flow # Needs to be burned to the ground and re-written; for now, diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 84815fdfbb..5aff6825be 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -94,6 +94,11 @@ __BEGIN_DECLS */ #define PWM_LOWEST_MAX 1700 +/** + * Do not output a channel with this value + */ +#define PWM_IGNORE_THIS_CHANNEL UINT16_MAX + /** * Servo output signal type, value is actual servo output pulse * width in microseconds. diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h index 76ec55c3e8..ab640837bb 100644 --- a/src/drivers/drv_px4flow.h +++ b/src/drivers/drv_px4flow.h @@ -46,37 +46,6 @@ #define PX4FLOW_DEVICE_PATH "/dev/px4flow" -/** - * @addtogroup topics - * @{ - */ - -/** - * Optical flow in NED body frame in SI units. - * - * @see http://en.wikipedia.org/wiki/International_System_of_Units - * - * @warning If possible the usage of the raw flow and performing rotation-compensation - * using the autopilot angular rate estimate is recommended. - */ -struct px4flow_report { - - uint64_t timestamp; /**< in microseconds since system start */ - - int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */ - int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */ - float flow_comp_x_m; /**< speed over ground in meters per second, rotation-compensated */ - float flow_comp_y_m; /**< speed over ground in meters per second, rotation-compensated */ - float ground_distance_m; /**< Altitude / distance to ground in meters */ - uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ - uint8_t sensor_id; /**< id of the sensor emitting the flow value */ - -}; - -/** - * @} - */ - /* * ObjDev tag for px4flow data. */ diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index f214b5964f..60ad3c1aff 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -37,7 +37,7 @@ * * Driver for the PX4FLOW module connected via I2C. */ - + #include #include @@ -68,7 +68,7 @@ #include #include -//#include +#include #include @@ -80,7 +80,7 @@ /* PX4FLOW Registers addresses */ #define PX4FLOW_REG 0x00 /* Measure Register */ -#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz +#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -115,17 +115,17 @@ class PX4FLOW : public device::I2C public: PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); virtual ~PX4FLOW(); - + virtual int init(); - + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - + /** * Diagnostics - print some basic information about the driver. */ void print_info(); - + protected: virtual int probe(); @@ -136,13 +136,13 @@ private: bool _sensor_ok; int _measure_ticks; bool _collect_phase; - + orb_advert_t _px4flow_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; - + /** * Test whether the device supported by the driver is present at a * specific address. @@ -151,7 +151,7 @@ private: * @return True if the device is present. */ int probe_address(uint8_t address); - + /** * Initialise the automatic measurement state machine and start it. * @@ -159,12 +159,12 @@ private: * to make it more aggressive about resetting the bus in case of errors. */ void start(); - + /** * Stop the automatic measurement state machine. */ void stop(); - + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. @@ -179,8 +179,8 @@ private: * @param arg Instance pointer for the driver that is polling. */ static void cycle_trampoline(void *arg); - - + + }; /* @@ -201,7 +201,7 @@ PX4FLOW::PX4FLOW(int bus, int address) : { // enable debug() calls _debug_enabled = true; - + // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); } @@ -226,13 +226,13 @@ PX4FLOW::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(px4flow_report)); + _reports = new RingBuffer(2, sizeof(struct optical_flow_s)); if (_reports == nullptr) goto out; /* get a publish handle on the px4flow topic */ - struct px4flow_report zero_report; + struct optical_flow_s zero_report; memset(&zero_report, 0, sizeof(zero_report)); _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report); @@ -323,24 +323,24 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) return -EINVAL; - + irqstate_t flags = irqsave(); if (!_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } irqrestore(flags); - + return OK; } case SENSORIOCGQUEUEDEPTH: return _reports->size(); - + case SENSORIOCRESET: /* XXX implement this */ return -EINVAL; - + default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); @@ -350,8 +350,8 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct px4flow_report); - struct px4flow_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct optical_flow_s); + struct optical_flow_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -425,7 +425,7 @@ PX4FLOW::measure() return ret; } ret = OK; - + return ret; } @@ -433,14 +433,14 @@ int PX4FLOW::collect() { int ret = -EIO; - + /* read from the sensor */ uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0}; - + perf_begin(_sample_perf); - + ret = transfer(nullptr, 0, &val[0], 22); - + if (ret < 0) { log("error reading from sensor: %d", ret); @@ -448,7 +448,7 @@ PX4FLOW::collect() perf_end(_sample_perf); return ret; } - + // f.frame_count = val[1] << 8 | val[0]; // f.pixel_flow_x_sum= val[3] << 8 | val[2]; // f.pixel_flow_y_sum= val[5] << 8 | val[4]; @@ -466,7 +466,7 @@ PX4FLOW::collect() int16_t flowcy = val[9] << 8 | val[8]; int16_t gdist = val[21] << 8 | val[20]; - struct px4flow_report report; + struct optical_flow_s report; report.flow_comp_x_m = float(flowcx)/1000.0f; report.flow_comp_y_m = float(flowcy)/1000.0f; report.flow_raw_x= val[3] << 8 | val[2]; @@ -503,7 +503,7 @@ PX4FLOW::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1); - + /* notify about state change */ struct subsystem_info_s info = { true, @@ -644,7 +644,7 @@ start() fail: - if (g_dev != nullptr) + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; @@ -678,7 +678,7 @@ void stop() void test() { - struct px4flow_report report; + struct optical_flow_s report; ssize_t sz; int ret; @@ -777,7 +777,7 @@ px4flow_main(int argc, char *argv[]) */ if (!strcmp(argv[1], "start")) px4flow::start(); - + /* * Stop the driver */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 82977a032f..122a3cd174 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1272,7 +1272,9 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) memcpy(values, buffer, count * 2); for (uint8_t i = 0; i < count; i++) { - up_pwm_servo_set(i, values[i]); + if (values[i] != PWM_IGNORE_THIS_CHANNEL) { + up_pwm_servo_set(i, values[i]); + } } return count * 2; diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index c555a0a692..9d479832f8 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -66,10 +66,10 @@ void CatapultLaunchMethod::update(float accel_x) last_timestamp = hrt_absolute_time(); if (accel_x > threshold_accel.get()) { - integrator += accel_x * dt; + integrator += dt; // warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", // (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); - if (integrator > threshold_accel.get() * threshold_time.get()) { + if (integrator > threshold_time.get()) { launchDetected = true; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 74deda8cca..a5a7728252 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -615,6 +615,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: case VEHICLE_CMD_PREFLIGHT_STORAGE: + case VEHICLE_CMD_CUSTOM_0: + case VEHICLE_CMD_CUSTOM_1: + case VEHICLE_CMD_CUSTOM_2: + case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: + case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: /* ignore commands that handled in low prio loop */ break; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 93f4fec92e..940e641445 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1396,7 +1396,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); - configure_stream("OPTICAL_FLOW", 0.5f); + configure_stream("OPTICAL_FLOW", 20.0f); break; case MAVLINK_MODE_ONBOARD: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 5a92004a61..a2f3828ffa 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -886,8 +886,8 @@ protected: msg.eph = cm_uint16_from_m_float(gps.eph); msg.epv = cm_uint16_from_m_float(gps.epv); msg.vel = cm_uint16_from_m_float(gps.vel_m_s), - msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, - msg.satellites_visible = gps.satellites_used; + msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + msg.satellites_visible = gps.satellites_used; _mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg); } @@ -957,11 +957,11 @@ protected: msg.lat = pos.lat * 1e7; msg.lon = pos.lon * 1e7; msg.alt = pos.alt * 1000.0f; - msg.relative_alt = (pos.alt - home.alt) * 1000.0f; - msg.vx = pos.vel_n * 100.0f; - msg.vy = pos.vel_e * 100.0f; - msg.vz = pos.vel_d * 100.0f; - msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; + msg.relative_alt = (pos.alt - home.alt) * 1000.0f; + msg.vx = pos.vel_n * 100.0f; + msg.vy = pos.vel_e * 100.0f; + msg.vz = pos.vel_d * 100.0f; + msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg); } @@ -1022,9 +1022,9 @@ protected: msg.x = pos.x; msg.y = pos.y; msg.z = pos.z; - msg.vx = pos.vx; - msg.vy = pos.vy; - msg.vz = pos.vz; + msg.vx = pos.vx; + msg.vy = pos.vy; + msg.vz = pos.vz; _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg); } @@ -1085,9 +1085,9 @@ protected: msg.x = pos.x; msg.y = pos.y; msg.z = pos.z; - msg.roll = pos.roll; - msg.pitch = pos.pitch; - msg.yaw = pos.yaw; + msg.roll = pos.roll; + msg.pitch = pos.pitch; + msg.yaw = pos.yaw; _mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg); } diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 43161aa70e..0da778b6f6 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -285,7 +285,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { /* XXX range-check value? */ - r_page_servos[offset] = *values; + if (*values != PWM_IGNORE_THIS_CHANNEL) { + r_page_servos[offset] = *values; + } offset++; num_values--; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6c4b494522..dbda8ea6f3 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -76,6 +76,7 @@ #include #include #include +#include #include #include #include @@ -934,6 +935,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; + struct vision_position_estimate vision_pos; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -984,6 +986,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_EST1_s log_EST1; struct log_PWR_s log_PWR; struct log_VICN_s log_VICN; + struct log_VISN_s log_VISN; struct log_GS0A_s log_GS0A; struct log_GS0B_s log_GS0B; struct log_GS1A_s log_GS1A; @@ -1013,6 +1016,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int gps_pos_sub; int sat_info_sub; int vicon_pos_sub; + int vision_pos_sub; int flow_sub; int rc_sub; int airspeed_sub; @@ -1043,6 +1047,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); + subs.vision_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate)); subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); subs.rc_sub = orb_subscribe(ORB_ID(rc_channels)); subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); @@ -1459,6 +1464,22 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw; LOGBUFFER_WRITE_AND_COUNT(VICN); } + + /* --- VISION POSITION --- */ + if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) { + log_msg.msg_type = LOG_VISN_MSG; + log_msg.body.log_VISN.x = buf.vision_pos.x; + log_msg.body.log_VISN.y = buf.vision_pos.y; + log_msg.body.log_VISN.z = buf.vision_pos.z; + log_msg.body.log_VISN.vx = buf.vision_pos.vx; + log_msg.body.log_VISN.vy = buf.vision_pos.vy; + log_msg.body.log_VISN.vz = buf.vision_pos.vz; + log_msg.body.log_VISN.qx = buf.vision_pos.q[0]; + log_msg.body.log_VISN.qy = buf.vision_pos.q[1]; + log_msg.body.log_VISN.qz = buf.vision_pos.q[2]; + log_msg.body.log_VISN.qw = buf.vision_pos.q[3]; + LOGBUFFER_WRITE_AND_COUNT(VISN); + } /* --- FLOW --- */ if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) { diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index fb7609435b..6741c7e258 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -391,6 +391,20 @@ struct log_TEL_s { uint64_t heartbeat_time; }; +/* --- VISN - VISION POSITION --- */ +#define LOG_VISN_MSG 38 +struct log_VISN_s { + float x; + float y; + float z; + float vx; + float vy; + float vz; + float qx; + float qy; + float qz; + float qw; +}; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -449,6 +463,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), + LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"), LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index 7db33d98b3..44aa505721 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier + * Copyright (C) 2012-2014 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 @@ -37,6 +34,10 @@ /** * @file vehicle_command.h * Definition of the vehicle command uORB topic. + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier */ #ifndef TOPIC_VEHICLE_COMMAND_H_ @@ -52,6 +53,9 @@ * but can contain additional ones. */ enum VEHICLE_CMD { + VEHICLE_CMD_CUSTOM_0 = 0, /* test command */ + VEHICLE_CMD_CUSTOM_1 = 1, /* test command */ + VEHICLE_CMD_CUSTOM_2 = 2, /* test command */ VEHICLE_CMD_NAV_WAYPOINT = 16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_LOITER_UNLIM = 17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_LOITER_TURNS = 18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ @@ -87,7 +91,8 @@ enum VEHICLE_CMD { VEHICLE_CMD_MISSION_START = 300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ VEHICLE_CMD_START_RX_PAIR = 500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - VEHICLE_CMD_ENUM_END = 501, /* | */ + VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001, /**< Prepare a payload deployment in the flight plan */ + VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 /**< Control a pre-programmed payload deployment */ }; /** @@ -115,8 +120,8 @@ struct vehicle_command_s { float param2; /**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum. */ float param3; /**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum. */ float param4; /**< Parameter 4, as defined by MAVLink VEHICLE_CMD enum. */ - float param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ - float param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ + double param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ + double param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */ enum VEHICLE_CMD command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */ uint8_t target_system; /**< System which should execute the command */ diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 8548660fe1..0d67aad476 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -70,6 +70,16 @@ unsigned UavcanGnssBridge::get_num_redundant_channels() const return (_receiver_node_id < 0) ? 0 : 1; } +void UavcanGnssBridge::print_status() const +{ + printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount()); + if (_receiver_node_id < 0) { + printf("N/A\n"); + } else { + printf("%d\n", _receiver_node_id); + } +} + void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) { // This bridge does not support redundant GNSS receivers yet. diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index c2b6e41956..e8466b4010 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -66,6 +66,8 @@ public: unsigned get_num_redundant_channels() const override; + void print_status() const override; + private: /** * GNSS fix message will be reported via this callback. diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index a98596f9c5..9608ce6804 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -58,7 +58,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List= 0) { + if (_channels[i].node_id >= 0) { (void)unregister_class_devname(_class_devname, _channels[i].class_instance); } } @@ -66,13 +66,15 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() delete [] _channels; } -void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const void *report) +void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) { + assert(report != nullptr); + Channel *channel = nullptr; // Checking if such channel already exists for (unsigned i = 0; i < _max_channels; i++) { - if (_channels[i].redunancy_channel_id == redundancy_channel_id) { + if (_channels[i].node_id == node_id) { channel = _channels + i; break; } @@ -84,11 +86,11 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const return; // Give up immediately - saves some CPU time } - log("adding channel %d...", redundancy_channel_id); + log("adding channel %d...", node_id); // Search for the first free channel for (unsigned i = 0; i < _max_channels; i++) { - if (_channels[i].redunancy_channel_id < 0) { + if (_channels[i].node_id < 0) { channel = _channels + i; break; } @@ -111,9 +113,9 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const } // Publish to the appropriate topic, abort on failure - channel->orb_id = _orb_topics[class_instance]; - channel->redunancy_channel_id = redundancy_channel_id; - channel->class_instance = class_instance; + channel->orb_id = _orb_topics[class_instance]; + channel->node_id = node_id; + channel->class_instance = class_instance; channel->orb_advert = orb_advertise(channel->orb_id, report); if (channel->orb_advert < 0) { @@ -123,7 +125,7 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const return; } - log("channel %d class instance %d ok", channel->redunancy_channel_id, channel->class_instance); + log("channel %d class instance %d ok", channel->node_id, channel->class_instance); } assert(channel != nullptr); @@ -134,9 +136,23 @@ unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const { unsigned out = 0; for (unsigned i = 0; i < _max_channels; i++) { - if (_channels[i].redunancy_channel_id >= 0) { + if (_channels[i].node_id >= 0) { out += 1; } } return out; } + +void UavcanCDevSensorBridgeBase::print_status() const +{ + printf("devname: %s\n", _class_devname); + + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].node_id >= 0) { + printf("channel %d: node id %d --> class instance %d\n", + i, _channels[i].node_id, _channels[i].class_instance); + } else { + printf("channel %d: empty\n", i); + } + } +} diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index a13d0ab35d..1316f7ecc5 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -68,6 +68,11 @@ public: */ virtual unsigned get_num_redundant_channels() const = 0; + /** + * Prints current status in a human readable format to stdout. + */ + virtual void print_status() const = 0; + /** * Sensor bridge factory. * Creates a bridge object by its ASCII name, e.g. "gnss", "mag". @@ -84,7 +89,7 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD { struct Channel { - int redunancy_channel_id = -1; + int node_id = -1; orb_id_t orb_id = nullptr; orb_advert_t orb_advert = -1; int class_instance = -1; @@ -112,13 +117,15 @@ protected: /** * Sends one measurement into appropriate ORB topic. * New redundancy channels will be registered automatically. - * @param redundancy_channel_id Redundant sensor identifier (e.g. UAVCAN Node ID) - * @param report ORB message object + * @param node_id Sensor's Node ID + * @param report Pointer to ORB message object */ - void publish(const int redundancy_channel_id, const void *report); + void publish(const int node_id, const void *report); public: virtual ~UavcanCDevSensorBridgeBase(); unsigned get_num_redundant_channels() const override; + + void print_status() const override; }; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 482fec1e0a..95c6ba13e0 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -548,14 +548,16 @@ UavcanNode::print_info() (void)pthread_mutex_lock(&_node_mutex); // ESC mixer status - warnx("ESC actuators control groups: sub: %u / req: %u / fds: %u", - (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); - warnx("ESC mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); + printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n", + (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); + printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK"); // Sensor bridges auto br = _sensor_bridges.getHead(); while (br != nullptr) { - warnx("Sensor '%s': channels: %u", br->get_name(), br->get_num_redundant_channels()); + printf("Sensor '%s':\n", br->get_name()); + br->print_status(); + printf("\n"); br = br->getSibling(); }