fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 16:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged PX4Flow driver changes

Browse Source
This commit is contained in:
Lorenz Meier
2014-11-26 07:43:19 +01:00
parent 739c407cfd 4724c05047
commit cbcd1ea1d1
155 changed files with 2152 additions and 765 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink.c
+1 -5
View File
@@ -78,11 +78,7 @@ PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
mavlink_system_t mavlink_system = {
100,
50,
MAV_TYPE_FIXED_WING,
0,
0,
0
50
}; // System ID, 1-255, Component/Subsystem ID, 1-255
/*
src/modules/mavlink/mavlink_main.cpp
+8 -6
View File
@@ -167,8 +167,10 @@ Mavlink::Mavlink() :
_param_initialized(false),
_param_system_id(0),
_param_component_id(0),
_param_system_type(0),
_param_system_type(MAV_TYPE_FIXED_WING),
_param_use_hil_gps(0),
_param_forward_externalsp(0),
_system_type(0),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
@@ -524,7 +526,7 @@ void Mavlink::mavlink_update_system(void)
param_get(_param_system_type, &system_type);
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
mavlink_system.type = system_type;
_system_type = system_type;
}
int32_t use_hil_gps;
@@ -755,7 +757,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg)
pthread_mutex_lock(&_send_mutex);
int buf_free = get_free_tx_buf();
unsigned buf_free = get_free_tx_buf();
uint8_t payload_len = mavlink_message_lengths[msgid];
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
@@ -820,14 +822,14 @@ Mavlink::resend_message(mavlink_message_t *msg)
pthread_mutex_lock(&_send_mutex);
int buf_free = get_free_tx_buf();
unsigned buf_free = get_free_tx_buf();
_last_write_try_time = hrt_absolute_time();
unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
/* check if there is space in the buffer, let it overflow else */
if (buf_free < TX_BUFFER_GAP) {
if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) {
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
@@ -1634,7 +1636,7 @@ Mavlink::start(int argc, char *argv[])
task_spawn_cmd(buf,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2700,
2800,
(main_t)&Mavlink::start_helper,
(const char **)argv);
src/modules/mavlink/mavlink_main.h
+4
View File
@@ -265,6 +265,8 @@ public:
struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; }
unsigned get_system_type() { return _system_type; }
protected:
Mavlink *next;
@@ -354,6 +356,8 @@ private:
param_t _param_use_hil_gps;
param_t _param_forward_externalsp;
unsigned _system_type;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _txerr_perf; /**< TX error counter */
src/modules/mavlink/mavlink_messages.cpp
+57 -9
View File
@@ -302,7 +302,7 @@ protected:
msg.base_mode = 0;
msg.custom_mode = 0;
get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode);
msg.type = mavlink_system.type;
msg.type = _mavlink->get_system_type();
msg.autopilot = MAV_AUTOPILOT_PX4;
msg.mavlink_version = 3;
@@ -382,11 +382,11 @@ protected:
clock_gettime(CLOCK_REALTIME, &ts);
/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
struct tm t;
gmtime_r(&gps_time_sec, &t);
struct tm tt;
gmtime_r(&gps_time_sec, &tt);
// XXX we do not want to interfere here with the SD log app
strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t);
strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt);
snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name);
fp = fopen(log_file_path, "ab");
}
@@ -1353,15 +1353,17 @@ protected:
const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
unsigned system_type = _mavlink->get_system_type();
/* scale outputs depending on system type */
if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
mavlink_system.type == MAV_TYPE_HEXAROTOR ||
mavlink_system.type == MAV_TYPE_OCTOROTOR) {
if (system_type == MAV_TYPE_QUADROTOR ||
system_type == MAV_TYPE_HEXAROTOR ||
system_type == MAV_TYPE_OCTOROTOR) {
/* multirotors: set number of rotor outputs depending on type */
unsigned n;
switch (mavlink_system.type) {
switch (system_type) {
case MAV_TYPE_QUADROTOR:
n = 4;
break;
@@ -1717,7 +1719,53 @@ protected:
msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX;
msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX;
msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX;
msg.rssi = rc.rssi;
/* RSSI has a max value of 100, and when Spektrum or S.BUS are
* available, the RSSI field is invalid, as they do not provide
* an RSSI measurement. Use an out of band magic value to signal
* these digital ports. XXX revise MAVLink spec to address this.
* One option would be to use the top bit to toggle between RSSI
* and input source mode.
*
* Full RSSI field: 0b 1 111 1111
*
* ^ If bit is set, RSSI encodes type + RSSI
*
* ^ These three bits encode a total of 8
* digital RC input types.
* 0: PPM, 1: SBUS, 2: Spektrum, 2: ST24
* ^ These four bits encode a total of
* 16 RSSI levels. 15 = full, 0 = no signal
*
*/
/* Initialize RSSI with the special mode level flag */
msg.rssi = (1 << 7);
/* Set RSSI */
msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15;
switch (rc.input_source) {
case RC_INPUT_SOURCE_PX4FMU_PPM:
/* fallthrough */
case RC_INPUT_SOURCE_PX4IO_PPM:
msg.rssi |= (0 << 4);
break;
case RC_INPUT_SOURCE_PX4IO_SPEKTRUM:
msg.rssi |= (1 << 4);
break;
case RC_INPUT_SOURCE_PX4IO_SBUS:
msg.rssi |= (2 << 4);
break;
case RC_INPUT_SOURCE_PX4IO_ST24:
msg.rssi |= (3 << 4);
break;
}
if (rc.rc_lost) {
/* RSSI is by definition zero */
msg.rssi = 0;
}
_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg);
}
src/modules/mavlink/mavlink_receiver.cpp
+9 -5
View File
@@ -546,12 +546,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
offboard_control_sp.ignore &= ~(1 << i);
offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i));
}
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW);
offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) <<
OFB_IGN_BIT_YAW;
if (set_position_target_local_ned.type_mask & (1 << 10)) {
offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAW);
}
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE);
offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) <<
OFB_IGN_BIT_YAWRATE;
if (set_position_target_local_ned.type_mask & (1 << 11)) {
offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE);
}
offboard_control_sp.timestamp = hrt_absolute_time();
@@ -1404,7 +1408,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
struct sched_param param;
(void)pthread_attr_getschedparam(&receiveloop_attr, &param);
param.sched_priority = SCHED_PRIORITY_MAX - 40;
param.sched_priority = SCHED_PRIORITY_MAX - 80;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
pthread_attr_setstacksize(&receiveloop_attr, 2900);