From 76aa871a7125c98e79db4dde42de7863a24a762c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Jul 2014 00:26:33 +0200 Subject: estimator: Fixed body / world matrix initialization and reset --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 9622f7e40..deaaf55fe 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2503,12 +2503,12 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) // Calculate initial Tbn matrix and rotate Mag measurements into NED // to set initial NED magnetic field states - Mat3f DCM; - quat2Tbn(DCM, initQuat); + quat2Tbn(Tbn, initQuat); + Tnb = Tbn.transpose(); Vector3f initMagNED; - initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; - initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; - initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; + initMagNED.x = Tbn.x.x*initMagXYZ.x + Tbn.x.y*initMagXYZ.y + Tbn.x.z*initMagXYZ.z; + initMagNED.y = Tbn.y.x*initMagXYZ.x + Tbn.y.y*initMagXYZ.y + Tbn.y.z*initMagXYZ.z; + initMagNED.z = Tbn.z.x*initMagXYZ.x + Tbn.z.y*initMagXYZ.y + Tbn.z.z*initMagXYZ.z; magstate.q0 = initQuat[0]; magstate.q1 = initQuat[1]; @@ -2521,7 +2521,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) magstate.magYbias = magBias.y; magstate.magZbias = magBias.z; magstate.R_MAG = sq(magMeasurementSigma); - magstate.DCM = DCM; + magstate.DCM = Tbn; // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions -- cgit v1.2.3 From 1cca3ca8a5469e66dbb5bebbe518b55e4af426d8 Mon Sep 17 00:00:00 2001 From: Kynos Date: Thu, 3 Jul 2014 13:33:29 +0200 Subject: Use NAV-PVT with ubx7 and ubx8 modules This replaces NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC. --- src/drivers/gps/gps.cpp | 1 - src/drivers/gps/ubx.cpp | 125 +++++++++++++++++++++---- src/drivers/gps/ubx.h | 10 +- src/modules/mavlink/mavlink_receiver.cpp | 1 - src/modules/uORB/topics/vehicle_gps_position.h | 1 - 5 files changed, 113 insertions(+), 25 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 241e3bdf3..401b65dd4 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -299,7 +299,6 @@ GPS::task_main() _report_gps_pos.alt = (int32_t)1200e3f; _report_gps_pos.timestamp_variance = hrt_absolute_time(); _report_gps_pos.s_variance_m_s = 10.0f; - _report_gps_pos.p_variance_m = 10.0f; _report_gps_pos.c_variance_rad = 0.1f; _report_gps_pos.fix_type = 3; _report_gps_pos.eph = 0.9f; diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 44434a1df..d0854f5e9 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -95,7 +95,8 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct sate _got_velned(false), _disable_cmd_last(0), _ack_waiting_msg(0), - _ubx_version(0) + _ubx_version(0), + _use_nav_pvt(false) { decode_init(); } @@ -190,38 +191,45 @@ UBX::configure(unsigned &baudrate) /* configure message rates */ /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ - configure_message_rate(UBX_MSG_NAV_POSLLH, 1); + /* try to set rate for NAV-PVT */ + /* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */ + configure_message_rate(UBX_MSG_NAV_PVT, 1); if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; - } - - configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5); - - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; + _use_nav_pvt = false; + } else { + _use_nav_pvt = true; } + UBX_WARN("%susing NAV-PVT", _use_nav_pvt ? "" : "not "); - configure_message_rate(UBX_MSG_NAV_SOL, 1); + if (!_use_nav_pvt) { + configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; - } + configure_message_rate(UBX_MSG_NAV_POSLLH, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } - configure_message_rate(UBX_MSG_NAV_VELNED, 1); + configure_message_rate(UBX_MSG_NAV_SOL, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; + configure_message_rate(UBX_MSG_NAV_VELNED, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } } configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0); - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_MON_HW, 1); - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } @@ -454,11 +462,23 @@ UBX::payload_rx_init() _rx_state = UBX_RXMSG_HANDLE; // handle by default switch (_rx_msg) { + case UBX_MSG_NAV_PVT: + if ( (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */ + && (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) /* u-blox 8+ msg format */ + _rx_state = UBX_RXMSG_ERROR_LENGTH; + else if (!_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (!_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT + break; + case UBX_MSG_NAV_POSLLH: if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t)) _rx_state = UBX_RXMSG_ERROR_LENGTH; else if (!_configured) _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead break; case UBX_MSG_NAV_SOL: @@ -466,6 +486,8 @@ UBX::payload_rx_init() _rx_state = UBX_RXMSG_ERROR_LENGTH; else if (!_configured) _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead break; case UBX_MSG_NAV_TIMEUTC: @@ -473,6 +495,8 @@ UBX::payload_rx_init() _rx_state = UBX_RXMSG_ERROR_LENGTH; else if (!_configured) _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead break; case UBX_MSG_NAV_SVINFO: @@ -489,6 +513,8 @@ UBX::payload_rx_init() _rx_state = UBX_RXMSG_ERROR_LENGTH; else if (!_configured) _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead break; case UBX_MSG_MON_VER: @@ -675,6 +701,68 @@ UBX::payload_rx_done(void) // handle message switch (_rx_msg) { + case UBX_MSG_NAV_PVT: + UBX_TRACE_RXMSG("Rx NAV-PVT\n"); + + _gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType; + _gps_position->satellites_used = _buf.payload_rx_nav_pvt.numSV; + + _gps_position->lat = _buf.payload_rx_nav_pvt.lat; + _gps_position->lon = _buf.payload_rx_nav_pvt.lon; + _gps_position->alt = _buf.payload_rx_nav_pvt.hMSL; + + _gps_position->eph = (float)_buf.payload_rx_nav_pvt.hAcc * 1e-3f; + _gps_position->epv = (float)_buf.payload_rx_nav_pvt.vAcc * 1e-3f; + _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_pvt.sAcc * 1e-3f; + + _gps_position->vel_m_s = (float)_buf.payload_rx_nav_pvt.gSpeed * 1e-3f; + + _gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_pvt.velN * 1e-3f; + _gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_pvt.velE * 1e-3f; + _gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_pvt.velD * 1e-3f; + _gps_position->vel_ned_valid = true; + + _gps_position->cog_rad = (float)_buf.payload_rx_nav_pvt.headMot * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f; + + { + /* convert to unix timestamp */ + struct tm timeinfo; + timeinfo.tm_year = _buf.payload_rx_nav_pvt.year - 1900; + timeinfo.tm_mon = _buf.payload_rx_nav_pvt.month - 1; + timeinfo.tm_mday = _buf.payload_rx_nav_pvt.day; + timeinfo.tm_hour = _buf.payload_rx_nav_pvt.hour; + timeinfo.tm_min = _buf.payload_rx_nav_pvt.min; + timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec; + time_t epoch = mktime(&timeinfo); + +#ifndef CONFIG_RTC + //Since we lack a hardware RTC, set the system time clock based on GPS UTC + //TODO generalize this by moving into gps.cpp? + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; + clock_settime(CLOCK_REALTIME, &ts); +#endif + + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_pvt.nano * 1e-3f); + } + + _gps_position->timestamp_time = hrt_absolute_time(); + _gps_position->timestamp_velocity = hrt_absolute_time(); + _gps_position->timestamp_variance = hrt_absolute_time(); + _gps_position->timestamp_position = hrt_absolute_time(); + + _rate_count_vel++; + _rate_count_lat_lon++; + + _got_posllh = true; + _got_velned = true; + + ret = 1; + break; + case UBX_MSG_NAV_POSLLH: UBX_TRACE_RXMSG("Rx NAV-POSLLH\n"); @@ -697,7 +785,6 @@ UBX::payload_rx_done(void) _gps_position->fix_type = _buf.payload_rx_nav_sol.gpsFix; _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f; // from cm to m - _gps_position->p_variance_m = (float)_buf.payload_rx_nav_sol.pAcc * 1e-2f; // from cm to m _gps_position->satellites_used = _buf.payload_rx_nav_sol.numSV; _gps_position->timestamp_variance = hrt_absolute_time(); diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 65f2dd2ba..219a5762a 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -183,7 +183,7 @@ typedef struct { uint32_t reserved2; } ubx_payload_rx_nav_sol_t; -/* Rx NAV-PVT */ +/* Rx NAV-PVT (ubx8) */ typedef struct { uint32_t iTOW; /**< GPS Time of Week [ms] */ uint16_t year; /**< Year (UTC)*/ @@ -215,9 +215,11 @@ typedef struct { uint16_t pDOP; /**< Position DOP [0.01] */ uint16_t reserved2; uint32_t reserved3; - int32_t headVeh; /**< Heading of vehicle (2-D) [1e-5 deg] */ - uint32_t reserved4; + int32_t headVeh; /**< (ubx8+ only) Heading of vehicle (2-D) [1e-5 deg] */ + uint32_t reserved4; /**< (ubx8+ only) */ } ubx_payload_rx_nav_pvt_t; +#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7 (sizeof(ubx_payload_rx_nav_pvt_t) - 8) +#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8 (sizeof(ubx_payload_rx_nav_pvt_t)) /* Rx NAV-TIMEUTC */ typedef struct { @@ -395,6 +397,7 @@ typedef struct { /* General message and payload buffer union */ typedef union { + ubx_payload_rx_nav_pvt_t payload_rx_nav_pvt; ubx_payload_rx_nav_posllh_t payload_rx_nav_posllh; ubx_payload_rx_nav_sol_t payload_rx_nav_sol; ubx_payload_rx_nav_timeutc_t payload_rx_nav_timeutc; @@ -533,6 +536,7 @@ private: uint16_t _ack_waiting_msg; ubx_buf_t _buf; uint32_t _ubx_version; + bool _use_nav_pvt; }; #endif /* UBX_H_ */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 53a1638a3..6d361052c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -741,7 +741,6 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.timestamp_variance = timestamp; hil_gps.s_variance_m_s = 5.0f; - hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph; hil_gps.timestamp_velocity = timestamp; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index dbd59f4f3..80d65cd69 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -61,7 +61,6 @@ struct vehicle_gps_position_s { uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ - float p_variance_m; /**< position accuracy estimate m */ 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. */ -- cgit v1.2.3 From e97161e96b8de4031746556ab4bd4f96d24a30c8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 4 Jul 2014 15:53:26 +0200 Subject: mavlink: stack size for main thread increased --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 026a4d6c9..8dfa32ce8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2238,7 +2238,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1950, + 2700, (main_t)&Mavlink::start_helper, (const char **)argv); -- cgit v1.2.3 From d4eae37e478860a59e21f3caceb3d8fc28f9fa7c Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Fri, 4 Jul 2014 21:00:05 -0400 Subject: Commander: Only consider latest active data link heartbeat for timeout. --- src/modules/commander/commander.cpp | 30 ++++++++++++++++++------------ 1 file changed, 18 insertions(+), 12 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index efa26eb97..699ced1ab 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -767,6 +767,7 @@ int commander_thread_main(int argc, char *argv[]) hrt_abstime last_idle_time = 0; hrt_abstime start_time = 0; + hrt_abstime latest_heartbeat = 0; bool status_changed = true; bool param_init_forced = true; @@ -1367,19 +1368,24 @@ int commander_thread_main(int argc, char *argv[]) } /* data link check */ - if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { - /* handle the case where data link was regained */ - if (status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: data link regained"); - status.data_link_lost = false; - status_changed = true; - } + if (telemetry.heartbeat_time >= latest_heartbeat) { + if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* handle the case where data link was regained */ + if (status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: data link regained"); + status.data_link_lost = false; + status_changed = true; + } - } else { - if (!status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); - status.data_link_lost = true; - status_changed = true; + /* Only consider data link with most recent heartbeat */ + latest_heartbeat = telemetry.heartbeat_time; + + } else { + if (!status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); + status.data_link_lost = true; + status_changed = true; + } } } -- cgit v1.2.3 From b9299e68d4147845bab9ed99509b3e50b7c94ae1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 5 Jul 2014 13:35:12 -0700 Subject: Compiler warning fixes --- src/drivers/blinkm/blinkm.cpp | 2 +- src/lib/geo/geo.c | 14 ++++++------- src/modules/commander/commander.cpp | 8 ++++---- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 23 +++++++++++----------- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 4 ++-- src/modules/systemlib/cpuload.c | 2 +- src/modules/systemlib/systemlib.c | 3 +++ src/systemcmds/param/param.c | 5 +++-- 8 files changed, 33 insertions(+), 28 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 92e089217..6b14f5945 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -293,7 +293,7 @@ BlinkM::BlinkM(int bus, int blinkm) : safety_sub_fd(-1), num_of_cells(0), detected_cells_runcount(0), - t_led_color({0}), + t_led_color{0}, t_led_blink(0), led_thread_runcount(0), led_interval(1000), diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 212e33ff8..230be9281 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -297,12 +297,12 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do // TO DO - this is messed up and won't compile float start_disp_x = radius * sinf(arc_start_bearing); float start_disp_y = radius * cosf(arc_start_bearing); - float end_disp_x = radius * sinf(_wrapPI(arc_start_bearing + arc_sweep)); - float end_disp_y = radius * cosf(_wrapPI(arc_start_bearing + arc_sweep)); - float lon_start = lon_now + start_disp_x / 111111.0f; - float lat_start = lat_now + start_disp_y * cosf(lat_now) / 111111.0f; - float lon_end = lon_now + end_disp_x / 111111.0f; - float lat_end = lat_now + end_disp_y * cosf(lat_now) / 111111.0f; + float end_disp_x = radius * sinf(_wrapPI((double)(arc_start_bearing + arc_sweep))); + float end_disp_y = radius * cosf(_wrapPI((double)(arc_start_bearing + arc_sweep))); + float lon_start = (float)lon_now + start_disp_x / 111111.0f; + float lat_start = (float)lat_now + start_disp_y * cosf(lat_now) / 111111.0f; + float lon_end = (float)lon_now + end_disp_x / 111111.0f; + float lat_end = (float)lat_now + end_disp_y * cosf(lat_now) / 111111.0f; float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); @@ -319,7 +319,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do } - crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing); + crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing); return_value = OK; return return_value; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index efa26eb97..3bf3c857d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -472,7 +472,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. // We use an float epsilon delta to test float equality. if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { - mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1); + mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", (double)cmd->param1); } else { @@ -634,7 +634,7 @@ int commander_thread_main(int argc, char *argv[]) /* welcome user */ warnx("starting"); - char *main_states_str[MAIN_STATE_MAX]; + const char *main_states_str[MAIN_STATE_MAX]; main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; @@ -643,7 +643,7 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; main_states_str[MAIN_STATE_ACRO] = "ACRO"; - char *arming_states_str[ARMING_STATE_MAX]; + const char *arming_states_str[ARMING_STATE_MAX]; arming_states_str[ARMING_STATE_INIT] = "INIT"; arming_states_str[ARMING_STATE_STANDBY] = "STANDBY"; arming_states_str[ARMING_STATE_ARMED] = "ARMED"; @@ -652,7 +652,7 @@ int commander_thread_main(int argc, char *argv[]) arming_states_str[ARMING_STATE_REBOOT] = "REBOOT"; arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; - char *nav_states_str[NAVIGATION_STATE_MAX]; + const char *nav_states_str[NAVIGATION_STATE_MAX]; nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 000c02e3d..a9556f453 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -414,6 +414,17 @@ FixedwingPositionControl::FixedwingPositionControl() : _attitude_sp_pub(-1), _nav_capabilities_pub(-1), + _att(), + _att_sp(), + _nav_capabilities(), + _manual(), + _airspeed(), + _control_mode(), + _global_pos(), + _pos_sp_triplet(), + _sensor_combined(), + _range_finder(), + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), @@ -433,18 +444,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_valid(false), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - _att(), - _att_sp(), - _nav_capabilities(), - _manual(), - _airspeed(), - _control_mode(), - _global_pos(), - _pos_sp_triplet(), - _sensor_combined(), _mTecs(), - _was_pos_control_mode(false), - _range_finder() + _was_pos_control_mode(false) { _nav_capabilities.turn_distance = 0.0f; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index e4e405227..c22e60ae0 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -56,9 +56,9 @@ class BlockOutputLimiter: public SuperBlock { public: // methods - BlockOutputLimiter(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + BlockOutputLimiter(SuperBlock *parent, const char *name, bool fAngularLimit = false) : SuperBlock(parent, name), - _isAngularLimit(isAngularLimit), + _isAngularLimit(fAngularLimit), _min(this, "MIN"), _max(this, "MAX") {}; diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index ccc516f39..7aa2f3a5f 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -67,7 +67,7 @@ __EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pT __EXPORT struct system_load_s system_load; -extern FAR struct _TCB *sched_gettcb(pid_t pid); +extern FAR struct tcb_s *sched_gettcb(pid_t pid); void cpuload_initialize_once() { diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 9fff3eb88..90d8dd77c 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -54,6 +54,9 @@ #include "systemlib.h" +// Didn't seem right to include up_internal.h, so direct extern instead. +extern void up_systemreset(void) noreturn_function; + void systemreset(bool to_bootloader) { diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index f8bff2f6f..235ab7186 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -46,6 +46,7 @@ #include #include #include +#include #include #include @@ -228,8 +229,8 @@ do_show_print(void *arg, param_t param) if (!(arg == NULL)) { /* start search */ - char *ss = search_string; - char *pp = p_name; + const char *ss = search_string; + const char *pp = p_name; bool mismatch = false; /* XXX this comparison is only ok for trailing wildcards */ -- cgit v1.2.3 From bd5d3ebf70dc9e1aef106b60a840c17824d35b9b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Jul 2014 16:08:37 +0200 Subject: telemetry_statur: use 4 separate topics --- src/modules/commander/commander.cpp | 87 +++++++++++++++++++----------- src/modules/mavlink/mavlink_receiver.cpp | 86 +++++++++++++++-------------- src/modules/sdlog2/sdlog2.c | 31 ++++++----- src/modules/sdlog2/sdlog2_format.h | 8 +++ src/modules/sdlog2/sdlog2_messages.h | 35 +++++++----- src/modules/uORB/objects_common.cpp | 5 +- src/modules/uORB/topics/telemetry_status.h | 14 ++++- 7 files changed, 166 insertions(+), 100 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 699ced1ab..0003ec106 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -767,7 +767,6 @@ int commander_thread_main(int argc, char *argv[]) hrt_abstime last_idle_time = 0; hrt_abstime start_time = 0; - hrt_abstime latest_heartbeat = 0; bool status_changed = true; bool param_init_forced = true; @@ -797,10 +796,16 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); - /* Subscribe to telemetry status */ - int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); - struct telemetry_status_s telemetry; - memset(&telemetry, 0, sizeof(telemetry)); + /* Subscribe to telemetry status topics */ + int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; + uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; + bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; + + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + telemetry_last_heartbeat[i] = 0; + telemetry_lost[i] = true; + } /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -882,7 +887,6 @@ int commander_thread_main(int argc, char *argv[]) bool arming_state_changed = false; bool main_state_changed = false; bool failsafe_old = false; - bool system_checked = false; while (!thread_should_exit) { @@ -939,15 +943,6 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_datalink_loss, &datalink_loss_enabled); } - /* Perform system checks (again) once params are loaded and MAVLink is up. */ - if (!system_checked && mavlink_fd && - (telemetry.heartbeat_time > 0) && - (hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) { - - (void)rc_calibration_check(mavlink_fd); - system_checked = true; - } - orb_check(sp_man_sub, &updated); if (updated) { @@ -960,10 +955,26 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } - orb_check(telemetry_sub, &updated); + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + orb_check(telemetry_subs[i], &updated); - if (updated) { - orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry); + if (updated) { + struct telemetry_status_s telemetry; + memset(&telemetry, 0, sizeof(telemetry)); + + orb_copy(telemetry_status_orb_id[i], telemetry_subs[i], &telemetry); + + /* perform system checks when new telemetry link connected */ + if (mavlink_fd && + telemetry_last_heartbeat[i] == 0 && + telemetry.heartbeat_time > 0 && + hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) { + + (void)rc_calibration_check(mavlink_fd); + } + + telemetry_last_heartbeat[i] = telemetry.heartbeat_time; + } } orb_check(sensor_sub, &updated); @@ -1367,28 +1378,40 @@ int commander_thread_main(int argc, char *argv[]) } } - /* data link check */ - if (telemetry.heartbeat_time >= latest_heartbeat) { - if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* data links check */ + bool have_link = false; + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { /* handle the case where data link was regained */ - if (status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: data link regained"); - status.data_link_lost = false; - status_changed = true; + if (telemetry_lost[i]) { + mavlink_log_critical(mavlink_fd, "#audio: data link %i regained", i); + telemetry_lost[i] = false; } - - /* Only consider data link with most recent heartbeat */ - latest_heartbeat = telemetry.heartbeat_time; + have_link = true; } else { - if (!status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); - status.data_link_lost = true; - status_changed = true; + if (!telemetry_lost[i]) { + mavlink_log_critical(mavlink_fd, "#audio: data link %i lost", i); + telemetry_lost[i] = true; } } } + if (have_link) { + /* handle the case where data link was regained */ + if (status.data_link_lost) { + status.data_link_lost = false; + status_changed = true; + } + + } else { + if (!status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: ALL DATA LINKS LOST"); + status.data_link_lost = true; + status_changed = true; + } + } + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6d361052c..60da9c47d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -421,32 +421,35 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { - mavlink_radio_status_t rstatus; - mavlink_msg_radio_status_decode(msg, &rstatus); - - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); - - tstatus.timestamp = hrt_absolute_time(); - tstatus.heartbeat_time = _telemetry_heartbeat_time; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; - tstatus.rssi = rstatus.rssi; - tstatus.remote_rssi = rstatus.remrssi; - tstatus.txbuf = rstatus.txbuf; - tstatus.noise = rstatus.noise; - tstatus.remote_noise = rstatus.remnoise; - tstatus.rxerrors = rstatus.rxerrors; - tstatus.fixed = rstatus.fixed; - - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ + if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) { + mavlink_radio_status_t rstatus; + mavlink_msg_radio_status_decode(msg, &rstatus); + + struct telemetry_status_s tstatus; + memset(&tstatus, 0, sizeof(tstatus)); + + tstatus.timestamp = hrt_absolute_time(); + tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; + tstatus.rssi = rstatus.rssi; + tstatus.remote_rssi = rstatus.remrssi; + tstatus.txbuf = rstatus.txbuf; + tstatus.noise = rstatus.noise; + tstatus.remote_noise = rstatus.remnoise; + tstatus.rxerrors = rstatus.rxerrors; + tstatus.fixed = rstatus.fixed; + + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - } else { - orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); - } + } else { + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); + } - /* this means that heartbeats alone won't be published to the radio status no more */ - _radio_status_available = true; + /* this means that heartbeats alone won't be published to the radio status no more */ + _radio_status_available = true; + } } void @@ -475,28 +478,31 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) void MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) { - mavlink_heartbeat_t hb; - mavlink_msg_heartbeat_decode(msg, &hb); + /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ + if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) { + mavlink_heartbeat_t hb; + mavlink_msg_heartbeat_decode(msg, &hb); - /* ignore own heartbeats, accept only heartbeats from GCS */ - if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { - _telemetry_heartbeat_time = hrt_absolute_time(); + /* ignore own heartbeats, accept only heartbeats from GCS */ + if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { + _telemetry_heartbeat_time = hrt_absolute_time(); - /* if no radio status messages arrive, lets at least publish that heartbeats were received */ - if (!_radio_status_available) { + /* if no radio status messages arrive, lets at least publish that heartbeats were received */ + if (!_radio_status_available) { - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); + struct telemetry_status_s tstatus; + memset(&tstatus, 0, sizeof(tstatus)); - tstatus.timestamp = _telemetry_heartbeat_time; - tstatus.heartbeat_time = _telemetry_heartbeat_time; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; + tstatus.timestamp = _telemetry_heartbeat_time; + tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - } else { - orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + } else { + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); + } } } } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 39e5b6c41..0d36fa2c6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -979,7 +979,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GVSP_s log_GVSP; struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; - struct log_TELE_s log_TELE; + struct log_TEL_s log_TEL; struct log_EST0_s log_EST0; struct log_EST1_s log_EST1; struct log_PWR_s log_PWR; @@ -1019,7 +1019,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int esc_sub; int global_vel_sp_sub; int battery_sub; - int telemetry_sub; + int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; int range_finder_sub; int estimator_status_sub; int tecs_status_sub; @@ -1049,7 +1049,9 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); - subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + } subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); @@ -1479,16 +1481,19 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- TELEMETRY --- */ - if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) { - log_msg.msg_type = LOG_TELE_MSG; - log_msg.body.log_TELE.rssi = buf.telemetry.rssi; - log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi; - log_msg.body.log_TELE.noise = buf.telemetry.noise; - log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise; - log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors; - log_msg.body.log_TELE.fixed = buf.telemetry.fixed; - log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf; - LOGBUFFER_WRITE_AND_COUNT(TELE); + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + if (copy_if_updated(telemetry_status_orb_id[i], subs.telemetry_subs[i], &buf.telemetry)) { + log_msg.msg_type = LOG_TEL0_MSG + i; + log_msg.body.log_TEL.rssi = buf.telemetry.rssi; + log_msg.body.log_TEL.remote_rssi = buf.telemetry.remote_rssi; + log_msg.body.log_TEL.noise = buf.telemetry.noise; + log_msg.body.log_TEL.remote_noise = buf.telemetry.remote_noise; + log_msg.body.log_TEL.rxerrors = buf.telemetry.rxerrors; + log_msg.body.log_TEL.fixed = buf.telemetry.fixed; + log_msg.body.log_TEL.txbuf = buf.telemetry.txbuf; + log_msg.body.log_TEL.heartbeat_time = buf.telemetry.heartbeat_time; + LOGBUFFER_WRITE_AND_COUNT(TEL); + } } /* --- BOTTOM DISTANCE --- */ diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h index dc5e6c8bd..aff0e3f48 100644 --- a/src/modules/sdlog2/sdlog2_format.h +++ b/src/modules/sdlog2/sdlog2_format.h @@ -91,6 +91,14 @@ struct log_format_s { .labels = _labels \ } +#define LOG_FORMAT_S(_name, _struct_name, _format, _labels) { \ + .type = LOG_##_name##_MSG, \ + .length = sizeof(struct log_##_struct_name##_s) + LOG_PACKET_HEADER_LEN, \ + .name = #_name, \ + .format = _format, \ + .labels = _labels \ + } + #define LOG_FORMAT_MSG 0x80 #define LOG_PACKET_SIZE(_name) LOG_PACKET_HEADER_LEN + sizeof(struct log_##_name##_s) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 8c05e87c5..08a87e179 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -276,18 +276,7 @@ struct log_DIST_s { uint8_t flags; }; -/* --- TELE - TELEMETRY STATUS --- */ -#define LOG_TELE_MSG 22 -struct log_TELE_s { - uint8_t rssi; - uint8_t remote_rssi; - uint8_t noise; - uint8_t remote_noise; - uint16_t rxerrors; - uint16_t fixed; - uint8_t txbuf; -}; - +// ID 22 available // ID 23 available /* --- PWR - ONBOARD POWER SYSTEM --- */ @@ -385,6 +374,23 @@ struct log_EST1_s { float s[16]; }; +/* --- TEL0..3 - TELEMETRY STATUS --- */ +#define LOG_TEL0_MSG 34 +#define LOG_TEL1_MSG 35 +#define LOG_TEL2_MSG 36 +#define LOG_TEL3_MSG 37 +struct log_TEL_s { + uint8_t rssi; + uint8_t remote_rssi; + uint8_t noise; + uint8_t remote_noise; + uint16_t rxerrors; + uint16_t fixed; + uint8_t txbuf; + uint64_t heartbeat_time; +}; + + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -432,7 +438,10 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), - LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), + LOG_FORMAT_S(TEL0, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL1, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL2, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL3, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), 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"), diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 687fc1d4a..9b118205e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -186,7 +186,10 @@ ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); #include "topics/telemetry_status.h" -ORB_DEFINE(telemetry_status, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_0, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_1, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_2, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_3, struct telemetry_status_s); #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_s); diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index e9e00d76c..c4b99d520 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -72,6 +72,18 @@ struct telemetry_status_s { * @} */ -ORB_DECLARE(telemetry_status); +ORB_DECLARE(telemetry_status_0); +ORB_DECLARE(telemetry_status_1); +ORB_DECLARE(telemetry_status_2); +ORB_DECLARE(telemetry_status_3); + +#define TELEMETRY_STATUS_ORB_ID_NUM 4 + +static const struct orb_metadata *telemetry_status_orb_id[TELEMETRY_STATUS_ORB_ID_NUM] = { + ORB_ID(telemetry_status_0), + ORB_ID(telemetry_status_1), + ORB_ID(telemetry_status_2), + ORB_ID(telemetry_status_3), +}; #endif /* TOPIC_TELEMETRY_STATUS_H */ -- cgit v1.2.3 From 54b55c37c7ffec5c340f100d2027e91971967793 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Jul 2014 17:33:50 +0200 Subject: Reduce excessive stack sizes for FW estimation / control apps --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e4f805dc0..5d768b73d 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1533,7 +1533,7 @@ FixedwingEstimator::start() _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 6000, + 5000, (main_t)&FixedwingEstimator::task_main_trampoline, nullptr); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 000c02e3d..6c86544fa 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1449,7 +1449,7 @@ FixedwingPositionControl::start() _control_task = task_spawn_cmd("fw_pos_control_l1", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 4048, + 3500, (main_t)&FixedwingPositionControl::task_main_trampoline, nullptr); -- cgit v1.2.3 From 29eab09912ef38eca75b926d5006c0c59b4082b7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Jul 2014 17:58:21 +0200 Subject: sdlog2: TEL message format fixed --- src/modules/sdlog2/sdlog2_messages.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src/modules') diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 08a87e179..87f7f718f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -438,10 +438,10 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), - LOG_FORMAT_S(TEL0, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), - LOG_FORMAT_S(TEL1, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), - LOG_FORMAT_S(TEL2, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), - LOG_FORMAT_S(TEL3, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL2, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), 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"), -- cgit v1.2.3 From d2a2297a14bf2f4dbb71a6ed20c76617e93b14a2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 6 Jul 2014 11:24:39 -0700 Subject: Fixes #1130 --- src/modules/px4iofirmware/i2c.c | 3 +++ src/modules/px4iofirmware/px4io.h | 4 ++-- src/modules/px4iofirmware/registers.c | 2 +- src/modules/px4iofirmware/sbus.c | 4 ++-- 4 files changed, 8 insertions(+), 5 deletions(-) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 76762c0fc..6d1d1fc2d 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -64,12 +64,15 @@ #define rCCR REG(STM32_I2C_CCR_OFFSET) #define rTRISE REG(STM32_I2C_TRISE_OFFSET) +void i2c_reset(void); static int i2c_interrupt(int irq, void *context); static void i2c_rx_setup(void); static void i2c_tx_setup(void); static void i2c_rx_complete(void); static void i2c_tx_complete(void); +#ifdef DEBUG static void i2c_dump(void); +#endif static DMA_HANDLE rx_dma; static DMA_HANDLE tx_dma; diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index ca175bfbc..b00a96717 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -219,8 +219,8 @@ extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); -extern bool sbus1_output(uint16_t *values, uint16_t num_values); -extern bool sbus2_output(uint16_t *values, uint16_t num_values); +extern void sbus1_output(uint16_t *values, uint16_t num_values); +extern void sbus2_output(uint16_t *values, uint16_t num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 50108ea1b..b37259997 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -711,7 +711,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val { #define SELECT_PAGE(_page_name) \ do { \ - *values = &_page_name[0]; \ + *values = (uint16_t *)&_page_name[0]; \ *num_values = sizeof(_page_name) / sizeof(_page_name[0]); \ } while(0) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 70ccab180..0f0414148 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -116,14 +116,14 @@ sbus_init(const char *device) return sbus_fd; } -bool +void sbus1_output(uint16_t *values, uint16_t num_values) { char a = 'A'; write(sbus_fd, &a, 1); } -bool +void sbus2_output(uint16_t *values, uint16_t num_values) { char b = 'B'; -- cgit v1.2.3 From 2a79689224a381cf777c555d330f0a2aca8e3d9a Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 6 Jul 2014 16:51:06 -0700 Subject: Fix unused variable warnings --- src/modules/commander/commander.cpp | 4 ++-- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 7 +------ src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 2 +- src/modules/mavlink/mavlink_ftp.cpp | 2 +- src/systemcmds/tests/test_mathlib.cpp | 3 +++ 5 files changed, 8 insertions(+), 10 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f83c1caf8..3d4a297f7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -773,7 +773,7 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; - bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); + rc_calibration_check(mavlink_fd); /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -934,7 +934,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; /* re-check RC calibration */ - rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); + rc_calibration_check(mavlink_fd); } /* navigation parameters */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3e835cf81..3d6c62434 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -807,12 +807,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float eas2tas = 1.0f; // XXX calculate actual number based on current measurements - // XXX re-visit - float baro_altitude = _global_pos.alt; - /* filter speed and altitude for controller */ math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); - math::Vector<3> accel_earth = _R_nb * accel_body; float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; @@ -945,8 +941,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; - /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ - float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + /* Calculate altitude of last ordinary waypoint L */ float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f; float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index fc0a2551c..2e37d166e 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -220,7 +220,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight /* Apply overrride given by the limitOverride argument (this is used for limits which are not given by * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector * is running) */ - bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); + limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); /* Write part of the status message */ _status.flightPathAngleSp = flightPathAngleSp; diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index ca846a465..55a472bf6 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -346,7 +346,7 @@ MavlinkFTP::_workWrite(Request *req) MavlinkFTP::ErrorCode MavlinkFTP::_workRemove(Request *req) { - auto hdr = req->header(); + //auto hdr = req->header(); // for now, send error reply return kErrPerm; diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 00c649c75..df3ddb966 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -96,8 +96,11 @@ int test_mathlib(int argc, char *argv[]) TEST_OP("Vector<3> %% Vector<3>", v1 % v2); TEST_OP("Vector<3> length", v1.length()); TEST_OP("Vector<3> length squared", v1.length_squared()); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" TEST_OP("Vector<3> element read", volatile float a = v1(0)); TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]); +#pragma GCC diagnostic pop TEST_OP("Vector<3> element write", v1(0) = 1.0f); TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f); } -- cgit v1.2.3