diff options
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 33 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 18 | ||||
-rw-r--r-- | src/modules/uORB/objects_common.cpp | 3 | ||||
-rw-r--r-- | src/modules/uORB/topics/esc_status.h | 114 |
4 files changed, 167 insertions, 1 deletions
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 4cca46b9c..5cc80ead6 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,27 @@ 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_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..9afae6000 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -209,6 +209,23 @@ 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_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 +247,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, "HBBBHHHHHfH", "Counter,NumESC,Conn,No,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/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index e22b58cad..1135f1615 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -160,3 +160,6 @@ ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_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 |