mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-08 10:32:26 +08:00
Merge branch 'master' of github.com:PX4/Firmware
This commit is contained in:
Lorenz Meier
@@ -57,7 +57,7 @@ def main():
|
||||
for (root, dirs, files) in os.walk(args.folder):
|
||||
for file in files:
|
||||
# only prune text files
|
||||
if ".zip" in file or ".bin" in file or ".swp" in file:
|
||||
if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file:
|
||||
continue
|
||||
|
||||
file_path = os.path.join(root, file)
|
||||
|
||||
@@ -57,7 +57,8 @@
|
||||
typedef enum {
|
||||
GPS_DRIVER_MODE_NONE = 0,
|
||||
GPS_DRIVER_MODE_UBX,
|
||||
GPS_DRIVER_MODE_MTK
|
||||
GPS_DRIVER_MODE_MTK,
|
||||
GPS_DRIVER_MODE_ASHTECH
|
||||
} gps_driver_mode_t;
|
||||
|
||||
|
||||
|
||||
@@ -165,7 +165,7 @@ ORB_DECLARE(output_pwm);
|
||||
|
||||
#define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
|
||||
#define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
|
||||
#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
|
||||
#define DSMX8_BIND_PULSES 9 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
|
||||
|
||||
/** power up DSM receiver */
|
||||
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,96 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013. All rights reserved.
|
||||
* Author: Boriskin Aleksey <a.d.boriskin@gmail.com>
|
||||
* Kistanov Alexander <akistanov@gramant.ru>
|
||||
*
|
||||
* 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 ASHTECH protocol definitions */
|
||||
|
||||
#ifndef ASHTECH_H_
|
||||
#define ASHTECH_H_
|
||||
|
||||
#include "gps_helper.h"
|
||||
|
||||
#ifndef RECV_BUFFER_SIZE
|
||||
#define RECV_BUFFER_SIZE 512
|
||||
|
||||
#define SAT_INFO_MAX_SATELLITES 20
|
||||
#endif
|
||||
|
||||
|
||||
class ASHTECH : public GPS_Helper
|
||||
{
|
||||
enum ashtech_decode_state_t {
|
||||
NME_DECODE_UNINIT,
|
||||
NME_DECODE_GOT_SYNC1,
|
||||
NME_DECODE_GOT_ASTERIKS,
|
||||
NME_DECODE_GOT_FIRST_CS_BYTE
|
||||
};
|
||||
|
||||
int _fd;
|
||||
struct satellite_info_s *_satellite_info;
|
||||
struct vehicle_gps_position_s *_gps_position;
|
||||
int ashtechlog_fd;
|
||||
|
||||
ashtech_decode_state_t _decode_state;
|
||||
uint8_t _rx_buffer[RECV_BUFFER_SIZE];
|
||||
uint16_t _rx_buffer_bytes;
|
||||
bool _parse_error; /** parse error flag */
|
||||
char *_parse_pos; /** parse position */
|
||||
|
||||
bool _gsv_in_progress; /**< Indicates that gsv data parsing is in progress */
|
||||
/* int _satellites_count; **< Number of satellites info parsed. */
|
||||
uint8_t count; /**< Number of satellites in satellite info */
|
||||
uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */
|
||||
uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */
|
||||
uint8_t elevation[SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
|
||||
uint8_t azimuth[SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
|
||||
uint8_t snr[SAT_INFO_MAX_SATELLITES]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
|
||||
|
||||
public:
|
||||
ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info);
|
||||
~ASHTECH();
|
||||
int receive(unsigned timeout);
|
||||
int configure(unsigned &baudrate);
|
||||
void decode_init(void);
|
||||
int handle_message(int len);
|
||||
int parse_char(uint8_t b);
|
||||
/** Read int ASHTECH parameter */
|
||||
int32_t read_int();
|
||||
/** Read float ASHTECH parameter */
|
||||
double read_float();
|
||||
/** Read char ASHTECH parameter */
|
||||
char read_char();
|
||||
|
||||
};
|
||||
|
||||
#endif /* ASHTECH_H_ */
|
||||
@@ -69,6 +69,7 @@
|
||||
|
||||
#include "ubx.h"
|
||||
#include "mtk.h"
|
||||
#include "ashtech.h"
|
||||
|
||||
|
||||
#define TIMEOUT_5HZ 500
|
||||
@@ -341,6 +342,10 @@ GPS::task_main()
|
||||
_Helper = new MTK(_serial_fd, &_report_gps_pos);
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_ASHTECH:
|
||||
_Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
@@ -402,6 +407,10 @@ GPS::task_main()
|
||||
mode_str = "MTK";
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_ASHTECH:
|
||||
mode_str = "ASHTECH";
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
@@ -429,6 +438,10 @@ GPS::task_main()
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
_mode = GPS_DRIVER_MODE_ASHTECH;
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_ASHTECH:
|
||||
_mode = GPS_DRIVER_MODE_UBX;
|
||||
break;
|
||||
|
||||
@@ -475,6 +488,10 @@ GPS::print_info()
|
||||
warnx("protocol: MTK");
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_ASHTECH:
|
||||
warnx("protocol: ASHTECH");
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -40,6 +40,7 @@ MODULE_COMMAND = gps
|
||||
SRCS = gps.cpp \
|
||||
gps_helper.cpp \
|
||||
mtk.cpp \
|
||||
ashtech.cpp \
|
||||
ubx.cpp
|
||||
|
||||
MODULE_STACKSIZE = 1200
|
||||
|
||||
@@ -115,9 +115,9 @@ publish_gam_message(const uint8_t *buffer)
|
||||
|
||||
esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
|
||||
esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
|
||||
esc.esc[0].esc_temperature = msg.temperature1 - 20;
|
||||
esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
|
||||
esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));
|
||||
esc.esc[0].esc_temperature = static_cast<float>(msg.temperature1) - 20.0F;
|
||||
esc.esc[0].esc_voltage = static_cast<float>((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)) * 0.1F;
|
||||
esc.esc[0].esc_current = static_cast<float>((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1F;
|
||||
|
||||
/* announce the esc if needed, just publish else */
|
||||
if (_esc_pub > 0) {
|
||||
@@ -186,18 +186,18 @@ build_gam_response(uint8_t *buffer, size_t *size)
|
||||
msg.gam_sensor_id = GAM_SENSOR_ID;
|
||||
msg.sensor_text_id = GAM_SENSOR_TEXT_ID;
|
||||
|
||||
msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20);
|
||||
msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20.0F);
|
||||
msg.temperature2 = 20; // 0 deg. C.
|
||||
|
||||
uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage);
|
||||
const uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage * 10.0F);
|
||||
msg.main_voltage_L = (uint8_t)voltage & 0xff;
|
||||
msg.main_voltage_H = (uint8_t)(voltage >> 8) & 0xff;
|
||||
|
||||
uint16_t current = (uint16_t)(esc.esc[0].esc_current);
|
||||
const uint16_t current = (uint16_t)(esc.esc[0].esc_current * 10.0F);
|
||||
msg.current_L = (uint8_t)current & 0xff;
|
||||
msg.current_H = (uint8_t)(current >> 8) & 0xff;
|
||||
|
||||
uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f);
|
||||
const uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f);
|
||||
msg.rpm_L = (uint8_t)rpm & 0xff;
|
||||
msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff;
|
||||
|
||||
|
||||
@@ -600,8 +600,8 @@ MK::task_main()
|
||||
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_voltage = 0.0F;
|
||||
esc.esc[i].esc_current = static_cast<float>(Motor[i].Current) * 0.1F;
|
||||
esc.esc[i].esc_rpm = (uint16_t) 0;
|
||||
esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4;
|
||||
|
||||
@@ -614,7 +614,7 @@ MK::task_main()
|
||||
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_temperature = static_cast<float>(Motor[i].Temperature);
|
||||
esc.esc[i].esc_state = (uint16_t) Motor[i].State;
|
||||
esc.esc[i].esc_errorcount = (uint16_t) 0;
|
||||
|
||||
|
||||
@@ -546,7 +546,7 @@ SF0X::collect()
|
||||
float si_units;
|
||||
bool valid = false;
|
||||
|
||||
for (unsigned i = 0; i < ret; i++) {
|
||||
for (int i = 0; i < ret; i++) {
|
||||
if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) {
|
||||
valid = true;
|
||||
}
|
||||
|
||||
+10
-7
@@ -362,8 +362,12 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
|
||||
crosstrack_error->distance = 0.0f;
|
||||
crosstrack_error->bearing = 0.0f;
|
||||
|
||||
dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
|
||||
|
||||
// Return error if arguments are bad
|
||||
if (lat_now == 0.0 || lon_now == 0.0 || lat_start == 0.0 || lon_start == 0.0 || lat_end == 0.0d || lon_end == 0.0d) { return return_value; }
|
||||
if (dist_to_end < 0.1f) {
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
|
||||
bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
|
||||
@@ -377,7 +381,6 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
|
||||
return return_value;
|
||||
}
|
||||
|
||||
dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
|
||||
crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff);
|
||||
|
||||
if (sin(bearing_diff) >= 0) {
|
||||
@@ -414,10 +417,10 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
|
||||
crosstrack_error->bearing = 0.0f;
|
||||
|
||||
// Return error if arguments are bad
|
||||
if (lat_now == 0.0 || lon_now == 0.0 || lat_center == 0.0 || lon_center == 0.0 || radius == 0.0f) { return return_value; }
|
||||
if (radius < 0.1f) { return return_value; }
|
||||
|
||||
|
||||
if (arc_sweep >= 0) {
|
||||
if (arc_sweep >= 0.0f) {
|
||||
bearing_sector_start = arc_start_bearing;
|
||||
bearing_sector_end = arc_start_bearing + arc_sweep;
|
||||
|
||||
@@ -463,8 +466,8 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
|
||||
|
||||
double start_disp_x = (double)radius * sin(arc_start_bearing);
|
||||
double start_disp_y = (double)radius * cos(arc_start_bearing);
|
||||
double end_disp_x = (double)radius * sin(_wrapPI((double)(arc_start_bearing + arc_sweep)));
|
||||
double end_disp_y = (double)radius * cos(_wrapPI((double)(arc_start_bearing + arc_sweep)));
|
||||
double end_disp_x = (double)radius * sin(_wrap_pi((double)(arc_start_bearing + arc_sweep)));
|
||||
double end_disp_y = (double)radius * cos(_wrap_pi((double)(arc_start_bearing + arc_sweep)));
|
||||
double lon_start = lon_now + start_disp_x / 111111.0;
|
||||
double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0;
|
||||
double lon_end = lon_now + end_disp_x / 111111.0;
|
||||
@@ -484,7 +487,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
|
||||
|
||||
}
|
||||
|
||||
crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing);
|
||||
crosstrack_error->bearing = _wrap_pi((double)crosstrack_error->bearing);
|
||||
return_value = OK;
|
||||
return return_value;
|
||||
}
|
||||
|
||||
@@ -241,15 +241,15 @@ struct log_GPSP_s {
|
||||
#define LOG_ESC_MSG 18
|
||||
struct log_ESC_s {
|
||||
uint16_t counter;
|
||||
uint8_t esc_count;
|
||||
uint8_t esc_connectiontype;
|
||||
uint8_t esc_num;
|
||||
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_voltage;
|
||||
float esc_current;
|
||||
int32_t esc_rpm;
|
||||
float esc_temperature;
|
||||
float esc_setpoint;
|
||||
uint16_t esc_setpoint_raw;
|
||||
};
|
||||
@@ -452,7 +452,7 @@ static const struct log_format_s log_formats[] = {
|
||||
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
|
||||
LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"),
|
||||
LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
|
||||
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
|
||||
LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
|
||||
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
|
||||
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
|
||||
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
|
||||
|
||||
@@ -78,6 +78,7 @@ enum ESC_CONNECTION_TYPE {
|
||||
|
||||
/**
|
||||
* Electronic speed controller status.
|
||||
* Unsupported float fields should be assigned NaN.
|
||||
*/
|
||||
struct esc_status_s {
|
||||
/* use of a counter and timestamp recommended (but not necessary) */
|
||||
@@ -89,17 +90,17 @@ struct esc_status_s {
|
||||
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 */
|
||||
uint32_t esc_errorcount; /**< Number of reported errors by ESC - if supported */
|
||||
int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */
|
||||
float esc_voltage; /**< Voltage measured from current ESC [V] - if supported */
|
||||
float esc_current; /**< Current measured from current ESC [A] - if supported */
|
||||
float esc_temperature; /**< Temperature measured from current ESC [degC] - 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 */
|
||||
uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */
|
||||
uint16_t esc_version; /**< Version of current ESC - if supported */
|
||||
uint16_t esc_state; /**< State of ESC - depend on Vendor */
|
||||
} esc[CONNECTED_ESC_MAX];
|
||||
|
||||
};
|
||||
|
||||
@@ -62,7 +62,7 @@ struct vehicle_gps_position_s {
|
||||
uint64_t timestamp_variance;
|
||||
float s_variance_m_s; /**< speed accuracy estimate m/s */
|
||||
float c_variance_rad; /**< course accuracy estimate rad */
|
||||
uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
|
||||
uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
|
||||
|
||||
float eph; /**< GPS HDOP horizontal dilution of position in m */
|
||||
float epv; /**< GPS VDOP horizontal dilution of position in m */
|
||||
|
||||
@@ -73,7 +73,7 @@ int UavcanEscController::init()
|
||||
|
||||
void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
|
||||
{
|
||||
if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) {
|
||||
if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) {
|
||||
perf_count(_perfcnt_invalid_input);
|
||||
return;
|
||||
}
|
||||
@@ -93,25 +93,24 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
|
||||
*/
|
||||
uavcan::equipment::esc::RawCommand msg;
|
||||
|
||||
static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
|
||||
|
||||
if (_armed) {
|
||||
for (unsigned i = 0; i < num_outputs; i++) {
|
||||
|
||||
float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX;
|
||||
float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
|
||||
if (scaled < 1.0F) {
|
||||
scaled = 1.0F; // Since we're armed, we don't want to stop it completely
|
||||
}
|
||||
|
||||
if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) {
|
||||
scaled = uavcan::equipment::esc::RawCommand::CMD_MIN;
|
||||
if (scaled > cmd_max) {
|
||||
scaled = cmd_max;
|
||||
perf_count(_perfcnt_scaling_error);
|
||||
} else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) {
|
||||
scaled = uavcan::equipment::esc::RawCommand::CMD_MAX;
|
||||
perf_count(_perfcnt_scaling_error);
|
||||
} else {
|
||||
; // Correct value
|
||||
}
|
||||
|
||||
msg.cmd.push_back(static_cast<unsigned>(scaled));
|
||||
msg.cmd.push_back(static_cast<int>(scaled));
|
||||
|
||||
_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -129,10 +128,31 @@ void UavcanEscController::arm_esc(bool arm)
|
||||
|
||||
void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
|
||||
{
|
||||
// TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb()
|
||||
if (msg.esc_index < CONNECTED_ESC_MAX) {
|
||||
_esc_status.esc_count = uavcan::max<int>(_esc_status.esc_count, msg.esc_index + 1);
|
||||
_esc_status.timestamp = msg.getMonotonicTimestamp().toUSec();
|
||||
|
||||
auto &ref = _esc_status.esc[msg.esc_index];
|
||||
|
||||
ref.esc_address = msg.getSrcNodeID().get();
|
||||
|
||||
ref.esc_voltage = msg.voltage;
|
||||
ref.esc_current = msg.current;
|
||||
ref.esc_temperature = msg.temperature;
|
||||
ref.esc_setpoint = msg.power_rating_pct;
|
||||
ref.esc_rpm = msg.rpm;
|
||||
ref.esc_errorcount = msg.error_count;
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&)
|
||||
{
|
||||
// TODO publish to ORB
|
||||
_esc_status.counter += 1;
|
||||
_esc_status.esc_connectiontype = ESC_CONNECTION_TYPE_CAN;
|
||||
|
||||
if (_esc_status_pub > 0) {
|
||||
(void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status);
|
||||
} else {
|
||||
_esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -48,6 +48,8 @@
|
||||
#include <uavcan/equipment/esc/RawCommand.hpp>
|
||||
#include <uavcan/equipment/esc/Status.hpp>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
|
||||
|
||||
class UavcanEscController
|
||||
{
|
||||
@@ -73,9 +75,8 @@ private:
|
||||
void orb_pub_timer_cb(const uavcan::TimerEvent &event);
|
||||
|
||||
|
||||
static constexpr unsigned MAX_RATE_HZ = 100; ///< XXX make this configurable
|
||||
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5;
|
||||
static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize;
|
||||
static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable
|
||||
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10;
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanEscController*,
|
||||
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
|
||||
@@ -84,6 +85,10 @@ private:
|
||||
typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)>
|
||||
TimerCbBinder;
|
||||
|
||||
bool _armed = false;
|
||||
esc_status_s _esc_status = {};
|
||||
orb_advert_t _esc_status_pub = -1;
|
||||
|
||||
/*
|
||||
* libuavcan related things
|
||||
*/
|
||||
@@ -93,12 +98,6 @@ private:
|
||||
uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder> _uavcan_sub_status;
|
||||
uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer;
|
||||
|
||||
/*
|
||||
* ESC states
|
||||
*/
|
||||
bool _armed = false;
|
||||
uavcan::equipment::esc::Status _states[MAX_ESCS];
|
||||
|
||||
/*
|
||||
* Perf counters
|
||||
*/
|
||||
|
||||
+1
-1
Submodule uavcan updated: 01d5bb242a...4de0338824
Reference in New Issue
Block a user