From 1b6742cebe649fcdf55c2fa4b85b0eb30cf4c169 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Apr 2015 12:04:16 +0200 Subject: commander: Better user feedback after resolving preflight check warnings --- src/modules/commander/state_machine_helper.cpp | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) (limited to 'src') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 9aba61e14..bd31be7ac 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -211,7 +211,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s valid_transition = true; } - /* Sensors need to be initialized for STANDBY state */ + // Sensors need to be initialized for STANDBY state if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors not operational."); feedback_provided = true; @@ -219,15 +219,23 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } - /* Check if we are trying to arm, checks look good but we are in STANDBY_ERROR */ - if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR && - new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - if (status->condition_system_sensors_initialized) { - mavlink_and_console_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming"); - } else { - mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); + // Check if we are trying to arm, checks look good but we are in STANDBY_ERROR + if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { + + if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + + if (status->condition_system_sensors_initialized) { + mavlink_and_console_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming"); + } else { + mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); + } + feedback_provided = true; + + } else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && + status->condition_system_sensors_initialized) { + mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, power cycle to complete"); + feedback_provided = true; } - feedback_provided = true; } // Finish up the state transition -- cgit v1.2.3 From 36ca62ece9cf45eb5c0d4825825825d585f35373 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Apr 2015 14:01:42 +0200 Subject: param lib: Provide used index lookup --- src/modules/systemlib/param/param.c | 33 +++++++++++++++++++++++++++++++-- src/modules/systemlib/param/param.h | 8 ++++++++ 2 files changed, 39 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 8dea6e6cc..5dfef9dce 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -92,7 +92,7 @@ struct param_wbuf_s { }; // XXX this should be param_info_count, but need to work out linking -uint8_t param_changed_storage[(600 / sizeof(uint8_t)) + 1] = {}; +uint8_t param_changed_storage[(700 / sizeof(uint8_t)) + 1] = {}; /** flexible array holding modified parameter values */ UT_array *param_values; @@ -265,8 +265,37 @@ param_count_used(void) param_t param_for_index(unsigned index) { - if (index < param_info_count) + if (index < param_info_count) { return (param_t)index; + } + + return PARAM_INVALID; +} + +param_t +param_for_used_index(unsigned index) +{ + if (index < param_info_count) { + + /* walk all params and count */ + int count = 0; + + for (unsigned i = 0; i < (unsigned)param_info_count + 1; i++) { + for (unsigned j = 0; j < 8; j++) { + if (param_changed_storage[i] & (1 << j)) { + + /* we found the right used count, + * return the param value + */ + if (i == count) { + return (param_t)i; + } + + count++; + } + } + } + } return PARAM_INVALID; } diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index b29a7e51d..9cbe3570b 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -129,6 +129,14 @@ __EXPORT bool param_used(param_t param); */ __EXPORT param_t param_for_index(unsigned index); +/** + * Look up an used parameter by index. + * + * @param param The parameter to obtain the index for. + * @return The index of the parameter in use, or -1 if the parameter does not exist. + */ +__EXPORT param_t param_for_used_index(unsigned index); + /** * Look up the index of a parameter. * -- cgit v1.2.3 From 76ce611e842ce6daed69adb6fb23de7b5bb5b639 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Apr 2015 14:02:06 +0200 Subject: MAVLink app: Use right lookup function --- src/modules/mavlink/mavlink_parameters.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 20d7cfdbb..2a8b1fdec 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -130,7 +130,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) } else { /* when index is >= 0, send this parameter again */ - send_param(param_for_index(req_read.param_index)); + send_param(param_for_used_index(req_read.param_index)); } } break; @@ -192,6 +192,7 @@ MavlinkParametersManager::send(const hrt_abstime t) /* look for the first parameter which is used */ param_t p; do { + /* walk through all parameters, including unused ones */ p = param_for_index(_send_all_index); _send_all_index++; } while (p != PARAM_INVALID && !param_used(p)); -- cgit v1.2.3 From 8edbf72bd50417bcc14195ad34056e8c0e966581 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Apr 2015 14:02:22 +0200 Subject: sensors app: Use right lookup function --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 3fc8627c1..7b79d47a8 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1633,7 +1633,7 @@ Sensors::rc_parameter_map_poll(bool forced) /* Set the handle by index if the index is set, otherwise use the id */ if (_rc_parameter_map.param_index[i] >= 0) { - _parameter_handles.rc_param[i] = param_for_index((unsigned)_rc_parameter_map.param_index[i]); + _parameter_handles.rc_param[i] = param_for_used_index((unsigned)_rc_parameter_map.param_index[i]); } else { _parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]); -- cgit v1.2.3 From 0ebf626632675dc5d54d08164be04d37ec1d74d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Apr 2015 14:24:01 +0200 Subject: MAVLink app: Allow higher max data rate --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ba049bac4..22ff3edf6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -90,7 +90,7 @@ static const int ERROR = -1; #define DEFAULT_DEVICE_NAME "/dev/ttyS1" -#define MAX_DATA_RATE 20000 ///< max data rate in bytes/s +#define MAX_DATA_RATE 60000 ///< max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate #define FLOW_CONTROL_DISABLE_THRESHOLD 40 ///< picked so that some messages still would fit it. -- cgit v1.2.3 From bbb27b86352337d5e185386212f87664c568e20e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 01:30:35 +0200 Subject: commander: Reduce excessive stack size --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5e0346155..42338b832 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1170,7 +1170,7 @@ int commander_thread_main(int argc, char *argv[]) /* initialize low priority thread */ pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2100); + pthread_attr_setstacksize(&commander_low_prio_attr, 2000); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); -- cgit v1.2.3 From b1c0176f4250ca1b88271cf06402085beb395d0f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 01:31:13 +0200 Subject: MC pos control: Reduce stack --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 5191a4de3..6ffb37d97 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1426,7 +1426,7 @@ MulticopterPositionControl::start() _control_task = task_spawn_cmd("mc_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 1600, + 1500, (main_t)&MulticopterPositionControl::task_main_trampoline, nullptr); -- cgit v1.2.3 From eba97bba4c5562e5c1bc0611ea83dabb2bf0ce96 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 01:31:29 +0200 Subject: MC att control: reduce stack slightly --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 3a0dfe4c3..ec512ab56 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -908,7 +908,7 @@ MulticopterAttitudeControl::start() _control_task = task_spawn_cmd("mc_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 1600, + 1500, (main_t)&MulticopterAttitudeControl::task_main_trampoline, nullptr); -- cgit v1.2.3 From f7f949f4552fd0ecfaea7247dae0a87f800647f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 01:31:56 +0200 Subject: Navigator: Reduce excessive stack --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9691e26a8..042f926d3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -518,7 +518,7 @@ Navigator::start() _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 20, - 1800, + 1700, (main_t)&Navigator::task_main_trampoline, nullptr); -- cgit v1.2.3 From 28dfb8983a92848ae52472702fd91e2275b991dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 01:32:34 +0200 Subject: uORB: Add API to check if a topic exists yet --- src/modules/uORB/uORB.cpp | 20 ++++++++++++++++++++ src/modules/uORB/uORB.h | 9 +++++++++ 2 files changed, 29 insertions(+) (limited to 'src') diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index b4f81d429..d531c6b0b 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -1196,6 +1197,25 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve } // namespace +int +orb_exists(const struct orb_metadata *meta, int instance) +{ + /* + * Generate the path to the node and try to open it. + */ + char path[orb_maxpath]; + int inst = instance; + int ret = node_mkpath(path, PUBSUB, meta, &inst); + + if (ret != OK) { + errno = -ret; + return ERROR; + } + + struct stat buffer; + return stat(path, &buffer); +} + orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 44dc6614f..a8b19d91f 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -318,6 +318,15 @@ extern int orb_check(int handle, bool *updated) __EXPORT; */ extern int orb_stat(int handle, uint64_t *time) __EXPORT; +/** + * Check if a topic has already been created. + * + * @param meta ORB topic metadata. + * @param instance ORB instance + * @return OK if the topic exists, ERROR otherwise with errno set accordingly. + */ +extern int orb_exists(const struct orb_metadata *meta, int instance) __EXPORT; + /** * Return the priority of the topic * -- cgit v1.2.3 From 6e060c01a76401172e452e562993f79acfef9d1a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 01:33:09 +0200 Subject: SDLOG2: Optimize runtime efficiency --- src/modules/sdlog2/sdlog2.c | 178 ++++++++++++++++++++++---------------------- 1 file changed, 88 insertions(+), 90 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e2274342e..ae4913559 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -51,7 +51,6 @@ #include #include #include -#include #include #include #include @@ -151,11 +150,6 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1); log_msgs_skipped++; \ } -#define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \ - fds[fdsc_count].fd = subs.##_var##_sub; \ - fds[fdsc_count].events = POLLIN; \ - fdsc_count++; - #define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) static bool main_thread_should_exit = false; /**< Deamon exit flag */ @@ -218,7 +212,7 @@ static void *logwriter_thread(void *arg); */ __EXPORT int sdlog2_main(int argc, char *argv[]); -static bool copy_if_updated(orb_id_t topic, int handle, void *buffer); +static bool copy_if_updated(orb_id_t topic, int *handle, void *buffer); /** * Mainloop of sd log deamon. @@ -811,14 +805,25 @@ int write_parameters(int fd) return written; } -bool copy_if_updated(orb_id_t topic, int handle, void *buffer) +bool copy_if_updated(orb_id_t topic, int *handle, void *buffer) { - bool updated; - - orb_check(handle, &updated); + bool updated = false; + + if (*handle < 0) { + if (OK == orb_exists(topic, 0)) { + *handle = orb_subscribe(topic); + /* copy first data */ + if (*handle >= 0) { + orb_copy(topic, *handle, buffer); + updated = true; + } + } + } else { + orb_check(*handle, &updated); - if (updated) { - orb_copy(topic, handle, buffer); + if (updated) { + orb_copy(topic, *handle, buffer); + } } return updated; @@ -1121,54 +1126,47 @@ int sdlog2_thread_main(int argc, char *argv[]) int mc_att_ctrl_status_sub; } subs; - subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); - subs.vtol_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status)); - subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - subs.act_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs)); - subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - subs.act_controls_1_sub = orb_subscribe(ORB_ID(actuator_controls_1)); - subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - 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)); - 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.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)); - subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); - subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); - subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); - subs.tsync_sub = orb_subscribe(ORB_ID(time_offset)); - subs.mc_att_ctrl_status_sub = orb_subscribe(ORB_ID(mc_att_ctrl_status)); - - /* we need to rate-limit wind, as we do not need the full update rate */ - orb_set_interval(subs.wind_sub, 90); - subs.encoders_sub = orb_subscribe(ORB_ID(encoders)); + subs.cmd_sub = -1; + subs.status_sub = -1; + subs.vtol_status_sub = -1; + subs.gps_pos_sub = -1; + subs.sensor_sub = -1; + subs.att_sub = -1; + subs.att_sp_sub = -1; + subs.rates_sp_sub = -1; + subs.act_outputs_sub = -1; + subs.act_controls_sub = -1; + subs.act_controls_1_sub = -1; + subs.local_pos_sub = -1; + subs.local_pos_sp_sub = -1; + subs.global_pos_sub = -1; + subs.triplet_sub = -1; + subs.vicon_pos_sub = -1; + subs.vision_pos_sub = -1; + subs.flow_sub = -1; + subs.rc_sub = -1; + subs.airspeed_sub = -1; + subs.esc_sub = -1; + subs.global_vel_sp_sub = -1; + subs.battery_sub = -1; + subs.range_finder_sub = -1; + subs.estimator_status_sub = -1; + subs.tecs_status_sub = -1; + subs.system_power_sub = -1; + subs.servorail_status_sub = -1; + subs.wind_sub = -1; + subs.tsync_sub = -1; + subs.mc_att_ctrl_status_sub = -1; + subs.encoders_sub = -1; /* add new topics HERE */ - for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); - } - - if (_extended_logging) { - subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); - } else { - subs.sat_info_sub = 0; + for (unsigned i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + subs.telemetry_subs[i] = -1; } + + subs.sat_info_sub = -1; /* close non-needed fd's */ @@ -1216,12 +1214,12 @@ int sdlog2_thread_main(int argc, char *argv[]) usleep(sleep_delay); /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ - if (copy_if_updated(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd)) { + if (copy_if_updated(ORB_ID(vehicle_command), &subs.cmd_sub, &buf.cmd)) { handle_command(&buf.cmd); } /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ - bool status_updated = copy_if_updated(ORB_ID(vehicle_status), subs.status_sub, &buf_status); + bool status_updated = copy_if_updated(ORB_ID(vehicle_status), &subs.status_sub, &buf_status); if (status_updated) { if (log_when_armed) { @@ -1230,7 +1228,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- GPS POSITION - LOG MANAGEMENT --- */ - bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos); + bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos); if (gps_pos_updated && log_name_timestamp) { gps_time = buf_gps_pos.time_utc_usec; @@ -1261,7 +1259,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- VTOL VEHICLE STATUS --- */ - if(copy_if_updated(ORB_ID(vtol_vehicle_status), subs.vtol_status_sub, &buf.vtol_status)) { + if(copy_if_updated(ORB_ID(vtol_vehicle_status), &subs.vtol_status_sub, &buf.vtol_status)) { log_msg.msg_type = LOG_VTOL_MSG; log_msg.body.log_VTOL.airspeed_tot = buf.vtol_status.airspeed_tot; LOGBUFFER_WRITE_AND_COUNT(VTOL); @@ -1292,7 +1290,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- SATELLITE INFO - UNIT #1 --- */ if (_extended_logging) { - if (copy_if_updated(ORB_ID(satellite_info), subs.sat_info_sub, &buf.sat_info)) { + if (copy_if_updated(ORB_ID(satellite_info), &subs.sat_info_sub, &buf.sat_info)) { /* log the SNR of each satellite for a detailed view of signal quality */ unsigned sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0])); @@ -1338,7 +1336,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- SENSOR COMBINED --- */ - if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) { + if (copy_if_updated(ORB_ID(sensor_combined), &subs.sensor_sub, &buf.sensor)) { bool write_IMU = false; bool write_IMU1 = false; bool write_IMU2 = false; @@ -1480,7 +1478,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- ATTITUDE --- */ - if (copy_if_updated(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att)) { + if (copy_if_updated(ORB_ID(vehicle_attitude), &subs.att_sub, &buf.att)) { log_msg.msg_type = LOG_ATT_MSG; log_msg.body.log_ATT.q_w = buf.att.q[0]; log_msg.body.log_ATT.q_x = buf.att.q[1]; @@ -1499,7 +1497,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- ATTITUDE SETPOINT --- */ - if (copy_if_updated(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp)) { + if (copy_if_updated(ORB_ID(vehicle_attitude_setpoint), &subs.att_sp_sub, &buf.att_sp)) { log_msg.msg_type = LOG_ATSP_MSG; log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body; log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body; @@ -1513,7 +1511,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- RATES SETPOINT --- */ - if (copy_if_updated(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp)) { + if (copy_if_updated(ORB_ID(vehicle_rates_setpoint), &subs.rates_sp_sub, &buf.rates_sp)) { log_msg.msg_type = LOG_ARSP_MSG; log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll; log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch; @@ -1522,14 +1520,14 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- ACTUATOR OUTPUTS --- */ - if (copy_if_updated(ORB_ID(actuator_outputs), subs.act_outputs_sub, &buf.act_outputs)) { + if (copy_if_updated(ORB_ID(actuator_outputs), &subs.act_outputs_sub, &buf.act_outputs)) { log_msg.msg_type = LOG_OUT0_MSG; memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output)); LOGBUFFER_WRITE_AND_COUNT(OUT0); } /* --- ACTUATOR CONTROL --- */ - if (copy_if_updated(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls)) { + if (copy_if_updated(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &subs.act_controls_sub, &buf.act_controls)) { log_msg.msg_type = LOG_ATTC_MSG; log_msg.body.log_ATTC.roll = buf.act_controls.control[0]; log_msg.body.log_ATTC.pitch = buf.act_controls.control[1]; @@ -1539,7 +1537,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- ACTUATOR CONTROL FW VTOL --- */ - if(copy_if_updated(ORB_ID(actuator_controls_1),subs.act_controls_1_sub,&buf.act_controls)) { + if(copy_if_updated(ORB_ID(actuator_controls_1), &subs.act_controls_1_sub,&buf.act_controls)) { log_msg.msg_type = LOG_ATC1_MSG; log_msg.body.log_ATTC.roll = buf.act_controls.control[0]; log_msg.body.log_ATTC.pitch = buf.act_controls.control[1]; @@ -1549,7 +1547,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- LOCAL POSITION --- */ - if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) { + if (copy_if_updated(ORB_ID(vehicle_local_position), &subs.local_pos_sub, &buf.local_pos)) { log_msg.msg_type = LOG_LPOS_MSG; log_msg.body.log_LPOS.x = buf.local_pos.x; log_msg.body.log_LPOS.y = buf.local_pos.y; @@ -1575,7 +1573,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- LOCAL POSITION SETPOINT --- */ - if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp)) { + if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), &subs.local_pos_sp_sub, &buf.local_pos_sp)) { log_msg.msg_type = LOG_LPSP_MSG; log_msg.body.log_LPSP.x = buf.local_pos_sp.x; log_msg.body.log_LPSP.y = buf.local_pos_sp.y; @@ -1591,7 +1589,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- GLOBAL POSITION --- */ - if (copy_if_updated(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos)) { + if (copy_if_updated(ORB_ID(vehicle_global_position), &subs.global_pos_sub, &buf.global_pos)) { log_msg.msg_type = LOG_GPOS_MSG; log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7; log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7; @@ -1610,7 +1608,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- GLOBAL POSITION SETPOINT --- */ - if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) { + if (copy_if_updated(ORB_ID(position_setpoint_triplet), &subs.triplet_sub, &buf.triplet)) { if (buf.triplet.current.valid) { log_msg.msg_type = LOG_GPSP_MSG; @@ -1628,7 +1626,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- VICON POSITION --- */ - if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) { + if (copy_if_updated(ORB_ID(vehicle_vicon_position), &subs.vicon_pos_sub, &buf.vicon_pos)) { log_msg.msg_type = LOG_VICN_MSG; log_msg.body.log_VICN.x = buf.vicon_pos.x; log_msg.body.log_VICN.y = buf.vicon_pos.y; @@ -1640,7 +1638,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- VISION POSITION --- */ - if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) { + 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; @@ -1656,7 +1654,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- FLOW --- */ - if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) { + if (copy_if_updated(ORB_ID(optical_flow), &subs.flow_sub, &buf.flow)) { log_msg.msg_type = LOG_FLOW_MSG; log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m; log_msg.body.log_FLOW.gyro_temperature = buf.flow.gyro_temperature; @@ -1672,7 +1670,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- RC CHANNELS --- */ - if (copy_if_updated(ORB_ID(rc_channels), subs.rc_sub, &buf.rc)) { + if (copy_if_updated(ORB_ID(rc_channels), &subs.rc_sub, &buf.rc)) { log_msg.msg_type = LOG_RC_MSG; /* Copy only the first 8 channels of 14 */ memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel)); @@ -1682,7 +1680,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- AIRSPEED --- */ - if (copy_if_updated(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed)) { + if (copy_if_updated(ORB_ID(airspeed), &subs.airspeed_sub, &buf.airspeed)) { log_msg.msg_type = LOG_AIRS_MSG; log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s; log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s; @@ -1691,7 +1689,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- ESCs --- */ - if (copy_if_updated(ORB_ID(esc_status), subs.esc_sub, &buf.esc)) { + if (copy_if_updated(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; @@ -1711,7 +1709,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- GLOBAL VELOCITY SETPOINT --- */ - if (copy_if_updated(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp)) { + if (copy_if_updated(ORB_ID(vehicle_global_velocity_setpoint), &subs.global_vel_sp_sub, &buf.global_vel_sp)) { log_msg.msg_type = LOG_GVSP_MSG; log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx; log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy; @@ -1720,7 +1718,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- BATTERY --- */ - if (copy_if_updated(ORB_ID(battery_status), subs.battery_sub, &buf.battery)) { + if (copy_if_updated(ORB_ID(battery_status), &subs.battery_sub, &buf.battery)) { log_msg.msg_type = LOG_BATT_MSG; log_msg.body.log_BATT.voltage = buf.battery.voltage_v; log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v; @@ -1730,7 +1728,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- SYSTEM POWER RAILS --- */ - if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) { + if (copy_if_updated(ORB_ID(system_power), &subs.system_power_sub, &buf.system_power)) { log_msg.msg_type = LOG_PWR_MSG; log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v; log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected; @@ -1748,8 +1746,8 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- TELEMETRY --- */ - 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)) { + for (unsigned 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; @@ -1764,7 +1762,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- BOTTOM DISTANCE --- */ - if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) { + if (copy_if_updated(ORB_ID(sensor_range_finder), &subs.range_finder_sub, &buf.range_finder)) { log_msg.msg_type = LOG_DIST_MSG; log_msg.body.log_DIST.bottom = buf.range_finder.distance; log_msg.body.log_DIST.bottom_rate = 0.0f; @@ -1773,7 +1771,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- ESTIMATOR STATUS --- */ - if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { + if (copy_if_updated(ORB_ID(estimator_status), &subs.estimator_status_sub, &buf.estimator_status)) { log_msg.msg_type = LOG_EST0_MSG; unsigned maxcopy0 = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_EST0.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_EST0.s); memset(&(log_msg.body.log_EST0.s), 0, sizeof(log_msg.body.log_EST0.s)); @@ -1792,7 +1790,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- TECS STATUS --- */ - if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) { + if (copy_if_updated(ORB_ID(tecs_status), &subs.tecs_status_sub, &buf.tecs_status)) { log_msg.msg_type = LOG_TECS_MSG; log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp; log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered; @@ -1812,7 +1810,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- WIND ESTIMATE --- */ - if (copy_if_updated(ORB_ID(wind_estimate), subs.wind_sub, &buf.wind_estimate)) { + if (copy_if_updated(ORB_ID(wind_estimate), &subs.wind_sub, &buf.wind_estimate)) { log_msg.msg_type = LOG_WIND_MSG; log_msg.body.log_WIND.x = buf.wind_estimate.windspeed_north; log_msg.body.log_WIND.y = buf.wind_estimate.windspeed_east; @@ -1822,7 +1820,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- ENCODERS --- */ - if (copy_if_updated(ORB_ID(encoders), subs.encoders_sub, &buf.encoders)) { + if (copy_if_updated(ORB_ID(encoders), &subs.encoders_sub, &buf.encoders)) { log_msg.msg_type = LOG_ENCD_MSG; log_msg.body.log_ENCD.cnt0 = buf.encoders.counts[0]; log_msg.body.log_ENCD.vel0 = buf.encoders.velocity[0]; @@ -1832,14 +1830,14 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- TIMESYNC OFFSET --- */ - if (copy_if_updated(ORB_ID(time_offset), subs.tsync_sub, &buf.time_offset)) { + if (copy_if_updated(ORB_ID(time_offset), &subs.tsync_sub, &buf.time_offset)) { log_msg.msg_type = LOG_TSYN_MSG; log_msg.body.log_TSYN.time_offset = buf.time_offset.offset_ns; LOGBUFFER_WRITE_AND_COUNT(TSYN); } /* --- MULTIROTOR ATTITUDE CONTROLLER STATUS --- */ - if (copy_if_updated(ORB_ID(mc_att_ctrl_status), subs.mc_att_ctrl_status_sub, &buf.mc_att_ctrl_status)) { + if (copy_if_updated(ORB_ID(mc_att_ctrl_status), &subs.mc_att_ctrl_status_sub, &buf.mc_att_ctrl_status)) { log_msg.msg_type = LOG_MACS_MSG; log_msg.body.log_MACS.roll_rate_integ = buf.mc_att_ctrl_status.roll_rate_integ; log_msg.body.log_MACS.pitch_rate_integ = buf.mc_att_ctrl_status.pitch_rate_integ; -- cgit v1.2.3 From 6c9e5d1ecfa68da213d73adf279e8995f0fb28cb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 07:08:44 +0200 Subject: commander: Only subscribe to existing telemetry status publications --- src/modules/commander/commander.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 42338b832..57afcf19a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1032,7 +1032,7 @@ int commander_thread_main(int argc, char *argv[]) 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_subs[i] = -1; telemetry_last_heartbeat[i] = 0; telemetry_last_dl_loss[i] = 0; telemetry_lost[i] = true; @@ -1290,6 +1290,11 @@ int commander_thread_main(int argc, char *argv[]) } for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + + if (telemetry_subs[i] < 0 && (OK == orb_exists(telemetry_status_orb_id[i], 0))) { + telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + } + orb_check(telemetry_subs[i], &updated); if (updated) { -- cgit v1.2.3 From 1d283bf3c15b79ad2aa23ed3e2de7ff80f0261fe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 08:02:31 +0200 Subject: MAVLink app: Fix usage of static struct, make streams list const --- src/modules/mavlink/mavlink_messages.cpp | 2 +- src/modules/mavlink/mavlink_messages.h | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 276035aa2..7ddc52fd1 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2231,7 +2231,7 @@ protected: }; -StreamListItem *streams_list[] = { +const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static), new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static), diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index 7e4416609..5b6b9d633 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -56,6 +56,6 @@ public: ~StreamListItem() {}; }; -extern StreamListItem *streams_list[]; +extern const StreamListItem *streams_list[]; #endif /* MAVLINK_MESSAGES_H_ */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index faede15cb..c4e332bf1 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -822,7 +822,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ if (!(_offboard_control_mode.ignore_attitude)) { - static struct vehicle_attitude_setpoint_s att_sp = {}; + struct vehicle_attitude_setpoint_s att_sp = {}; att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); -- cgit v1.2.3 From f1ff61ec4fdfd98a570aa4842f41aba30183bb7a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 08:03:12 +0200 Subject: sensors app: Move a static member to being a class member --- src/modules/sensors/sensors.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 7b79d47a8..07c3f2ab7 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -246,6 +246,7 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; struct rc_parameter_map_s _rc_parameter_map; + float _param_rc_values[RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */ math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */ @@ -523,6 +524,7 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), + _param_rc_values{}, _board_rotation{}, _mag_rotation{}, @@ -1838,8 +1840,6 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) void Sensors::set_params_from_rc() { - static float param_rc_values[RC_PARAM_MAP_NCHAN] = {}; - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) @@ -1851,8 +1851,8 @@ Sensors::set_params_from_rc() float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); /* Check if the value has changed, * maybe we need to introduce a more aggressive limit here */ - if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) { - param_rc_values[i] = rc_val; + if (rc_val > _param_rc_values[i] + FLT_EPSILON || rc_val < _param_rc_values[i] - FLT_EPSILON) { + _param_rc_values[i] = rc_val; float param_val = math::constrain( _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val, _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]); -- cgit v1.2.3 From b7409635b7ff8d52c8f9420c4caa6d67462525c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 08:04:46 +0200 Subject: Mission feasibility checker: Do not use static where its not needed. --- src/modules/navigator/mission_feasibility_checker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 904f44238..949231b15 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -134,7 +134,7 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, { /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ for (size_t i = 0; i < nMissionItems; i++) { - static struct mission_item_s missionitem; + struct mission_item_s missionitem; const ssize_t len = sizeof(struct mission_item_s); if (dm_read(dm_current, i, &missionitem, len) != len) { -- cgit v1.2.3 From a7f88d97b819a73ce5b0e804df21f9c9dc1ebadd Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 25 Apr 2015 12:37:43 -0700 Subject: Sensor cal rework - cancel support - versioned cal messages - better still detection - better messaging --- .../commander/accelerometer_calibration.cpp | 80 ++--- src/modules/commander/airspeed_calibration.cpp | 106 +++---- src/modules/commander/calibration_messages.h | 30 +- src/modules/commander/calibration_routines.cpp | 141 ++++++--- src/modules/commander/calibration_routines.h | 71 +++-- src/modules/commander/commander.cpp | 35 +-- src/modules/commander/gyro_calibration.cpp | 325 ++++++++++++--------- src/modules/commander/mag_calibration.cpp | 190 ++++++------ src/modules/sensors/sensors.cpp | 24 +- 9 files changed, 572 insertions(+), 430 deletions(-) (limited to 'src') diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 409c2ea00..d0982b341 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -152,11 +152,10 @@ static const int ERROR = -1; static const char *sensor_name = "accel"; -int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors); -int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num); +calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors); +calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num); int mat_invert3(float src[3][3], float dst[3][3]); -int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g); -int accel_calibration_worker(detect_orientation_return orientation, void* worker_data); +calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g); /// Data passed to calibration worker routine typedef struct { @@ -171,7 +170,7 @@ int do_accel_calibration(int mavlink_fd) int fd; int32_t device_id[max_accel_sens]; - mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); struct accel_scale accel_scale = { 0.0f, @@ -202,7 +201,7 @@ int do_accel_calibration(int mavlink_fd) close(fd); if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s); } } @@ -210,13 +209,21 @@ int do_accel_calibration(int mavlink_fd) float accel_T[max_accel_sens][3][3]; unsigned active_sensors; + /* measure and calculate offsets & scales */ if (res == OK) { - /* measure and calculate offsets & scales */ - res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors); + calibrate_return cal_return = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors); + if (cal_return == calibrate_return_cancelled) { + // Cancel message already displayed, nothing left to do + return ERROR; + } else if (cal_return == calibrate_return_ok) { + res = OK; + } else { + res = ERROR; + } } if (res != OK || active_sensors == 0) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG); return ERROR; } @@ -263,7 +270,7 @@ int do_accel_calibration(int mavlink_fd) failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i]))); if (failed) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, i); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, i); return ERROR; } @@ -271,7 +278,7 @@ int do_accel_calibration(int mavlink_fd) fd = open(str, 0); if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "sensor does not exist"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] sensor does not exist"); res = ERROR; } else { res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); @@ -279,7 +286,7 @@ int do_accel_calibration(int mavlink_fd) } if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, i); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, i); } } @@ -288,41 +295,44 @@ int do_accel_calibration(int mavlink_fd) res = param_save_default(); if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); } - mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); } else { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); } + + // This give a chance for the log messages to go out of the queue before someone else stomps on then + sleep(1); return res; } -int accel_calibration_worker(detect_orientation_return orientation, void* data) +static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) { const unsigned samples_num = 3000; accel_worker_data_t* worker_data = (accel_worker_data_t*)(data); - mavlink_and_console_log_info(worker_data->mavlink_fd, "Hold still, starting to measure %s side", detect_orientation_str(orientation)); + mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation)); read_accelerometer_avg(worker_data->subs, worker_data->accel_ref, orientation, samples_num); - mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side result: [ %8.4f %8.4f %8.4f ]", detect_orientation_str(orientation), + mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side result: [%8.4f %8.4f %8.4f]", detect_orientation_str(orientation), (double)worker_data->accel_ref[0][orientation][0], (double)worker_data->accel_ref[0][orientation][1], (double)worker_data->accel_ref[0][orientation][2]); worker_data->done_count++; - mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * worker_data->done_count); + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count); - return OK; + return calibrate_return_ok; } -int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors) +calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors) { - int result = OK; + calibrate_return result = calibrate_return_ok; *active_sensors = 0; @@ -343,7 +353,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac for (unsigned i = 0; i < max_accel_sens; i++) { worker_data.subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i); if (worker_data.subs[i] < 0) { - result = ERROR; + result = calibrate_return_error; break; } @@ -353,8 +363,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac timestamps[i] = arp.timestamp; } - if (result == OK) { - result = calibrate_from_orientation(mavlink_fd, data_collected, accel_calibration_worker, &worker_data); + if (result == calibrate_return_ok) { + int cancel_sub = calibrate_cancel_subscribe(); + result = calibrate_from_orientation(mavlink_fd, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */); + calibrate_cancel_unsubscribe(cancel_sub); } /* close all subscriptions */ @@ -370,13 +382,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac } } - if (result == OK) { + if (result == calibrate_return_ok) { /* calculate offsets and transform matrix */ for (unsigned i = 0; i < (*active_sensors); i++) { result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); - if (result != OK) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error"); + if (result != calibrate_return_ok) { + mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: calibration calculation error"); break; } } @@ -388,7 +400,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac /* * Read specified number of accelerometer samples, calculate average and dispersion. */ -int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num) +calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num) { struct pollfd fds[max_accel_sens]; @@ -432,7 +444,7 @@ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_a } if (errcount > samples_num / 10) { - return ERROR; + return calibrate_return_error; } } @@ -442,7 +454,7 @@ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_a } } - return OK; + return calibrate_return_ok; } int mat_invert3(float src[3][3], float dst[3][3]) @@ -468,7 +480,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) return OK; } -int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g) +calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g) { /* calculate offsets */ for (unsigned i = 0; i < 3; i++) { @@ -490,7 +502,7 @@ int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_s float mat_A_inv[3][3]; if (mat_invert3(mat_A, mat_A_inv) != OK) { - return ERROR; + return calibrate_return_error; } /* copy results to accel_T */ @@ -501,5 +513,5 @@ int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_s } } - return OK; + return calibrate_return_ok; } diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 747d915ff..837a3f1e8 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -38,6 +38,7 @@ #include "airspeed_calibration.h" #include "calibration_messages.h" +#include "calibration_routines.h" #include "commander_helper.h" #include @@ -61,19 +62,20 @@ static const int ERROR = -1; static const char *sensor_name = "dpress"; -#define HUMAN_ASPD_CAL_FAILED_MSG "Calibration failed, see http://px4.io/help/aspd" - static void feedback_calibration_failed(int mavlink_fd) { sleep(5); - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); - mavlink_log_critical(mavlink_fd, HUMAN_ASPD_CAL_FAILED_MSG); + mavlink_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); } int do_airspeed_calibration(int mavlink_fd) { + int result = OK; + unsigned calibration_counter = 0; + const unsigned maxcount = 3000; + /* give directions */ - mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); const unsigned calibration_count = 2000; @@ -96,11 +98,13 @@ int do_airspeed_calibration(int mavlink_fd) paramreset_successful = true; } else { - mavlink_log_critical(mavlink_fd, "airspeed offset zero failed"); + mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed"); } close(fd); } + + int cancel_sub = calibrate_cancel_subscribe(); if (!paramreset_successful) { @@ -108,26 +112,26 @@ int do_airspeed_calibration(int mavlink_fd) float analog_scaling = 0.0f; param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { - mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd"); - close(diff_pres_sub); - return ERROR; + mavlink_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd"); + goto error_return; } /* set scaling offset parameter */ if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); - close(diff_pres_sub); - return ERROR; + mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + goto error_return; } } - unsigned calibration_counter = 0; - - mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); + mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind"); usleep(500 * 1000); while (calibration_counter < calibration_count) { + if (calibrate_cancel_check(mavlink_fd, cancel_sub)) { + goto error_return; + } + /* wait blocking for new data */ struct pollfd fds[1]; fds[0].fd = diff_pres_sub; @@ -142,14 +146,13 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count); + mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count); } } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ feedback_calibration_failed(mavlink_fd); - close(diff_pres_sub); - return ERROR; + goto error_return; } } @@ -161,16 +164,15 @@ int do_airspeed_calibration(int mavlink_fd) airscale.offset_pa = diff_pres_offset; if (fd_scale > 0) { if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { - mavlink_log_critical(mavlink_fd, "airspeed offset update failed"); + mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed"); } close(fd_scale); } if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); - close(diff_pres_sub); - return ERROR; + mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + goto error_return; } /* auto-save to EEPROM */ @@ -178,30 +180,31 @@ int do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); - mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); - close(diff_pres_sub); - return ERROR; + mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); + goto error_return; } } else { feedback_calibration_failed(mavlink_fd); - close(diff_pres_sub); - return ERROR; + goto error_return; } - mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset); + mavlink_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); - mavlink_log_critical(mavlink_fd, "Create airflow now"); + mavlink_log_critical(mavlink_fd, "[cal] Create airflow now"); calibration_counter = 0; - const unsigned maxcount = 3000; /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ while (calibration_counter < maxcount) { + if (calibrate_cancel_check(mavlink_fd, cancel_sub)) { + goto error_return; + } + /* wait blocking for new data */ struct pollfd fds[1]; fds[0].fd = diff_pres_sub; @@ -216,7 +219,7 @@ int do_airspeed_calibration(int mavlink_fd) if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 500 == 0) { - mavlink_log_info(mavlink_fd, "Create air pressure! (got %d, wanted: 50 Pa)", + mavlink_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_raw_pa); } continue; @@ -224,30 +227,26 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", + mavlink_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); - mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); - close(diff_pres_sub); + mavlink_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!"); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ diff_pres_offset = 0.0f; if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); - close(diff_pres_sub); - return ERROR; + mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + goto error_return; } /* save */ - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0); + mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0); (void)param_save_default(); - close(diff_pres_sub); - feedback_calibration_failed(mavlink_fd); - return ERROR; + goto error_return; } else { - mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)", + mavlink_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); break; } @@ -255,21 +254,30 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ feedback_calibration_failed(mavlink_fd); - close(diff_pres_sub); - return ERROR; + goto error_return; } } if (calibration_counter == maxcount) { feedback_calibration_failed(mavlink_fd); - close(diff_pres_sub); - return ERROR; + goto error_return; } - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); + mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); - mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); tune_neutral(true); + +normal_return: + calibrate_cancel_unsubscribe(cancel_sub); close(diff_pres_sub); - return OK; + + // This give a chance for the log messages to go out of the queue before someone else stomps on then + sleep(1); + + return result; + +error_return: + result = ERROR; + goto normal_return; } diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index 29f44dc36..2f6d02a72 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -42,17 +42,25 @@ #ifndef CALIBRATION_MESSAGES_H_ #define CALIBRATION_MESSAGES_H_ -#define CAL_STARTED_MSG "%s calibration: started" -#define CAL_DONE_MSG "%s calibration: done" -#define CAL_FAILED_MSG "%s calibration: failed" -#define CAL_PROGRESS_MSG "%s calibration: progress <%u>" +// The calibration message defines which begin with CAL_QGC_ are used by QGroundControl to run a state +// machine to provide visual feedback for calibration. As such, the text for them or semantics of when +// they are displayed cannot be modified without causing QGC to break. If modifications are made, make +// sure to bump the calibration version number which will cause QGC to perform log based calibration +// instead of visual calibration until such a time as QGC is update to the new version. -#define CAL_FAILED_UNKNOWN_ERROR "ERROR: unknown error" -#define CAL_FAILED_SENSOR_MSG "ERROR: failed reading sensor" -#define CAL_FAILED_RESET_CAL_MSG "ERROR: failed to reset calibration, sensor %u" -#define CAL_FAILED_APPLY_CAL_MSG "ERROR: failed to apply calibration, sensor %u" -#define CAL_FAILED_SET_PARAMS_MSG "ERROR: failed to set parameters, sensor %u" -#define CAL_FAILED_SAVE_PARAMS_MSG "ERROR: failed to save parameters" -#define CAL_FAILED_ORIENTATION_TIMEOUT "ERROR: timed out waiting for correct orientation" +// The number in the cal started message is used to indicate the version stamp for the current calibration code. +#define CAL_QGC_STARTED_MSG "[cal] calibration started: 1 %s" +#define CAL_QGC_DONE_MSG "[cal] calibration done: %s" +#define CAL_QGC_FAILED_MSG "[cal] calibration failed: %s" +#define CAL_QGC_CANCELLED_MSG "[cal] calibration cancelled" +#define CAL_QGC_PROGRESS_MSG "[cal] progress <%u>" +#define CAL_QGC_ORIENTATION_DETECTED_MSG "[cal] %s orientation detected" +#define CAL_QGC_SIDE_DONE_MSG "[cal] %s side done, rotate to a different side" + +#define CAL_ERROR_SENSOR_MSG "[cal] ERROR: failed reading sensor" +#define CAL_ERROR_RESET_CAL_MSG "[cal] ERROR: failed to reset calibration, sensor %u" +#define CAL_ERROR_APPLY_CAL_MSG "[cal] ERROR: failed to apply calibration, sensor %u" +#define CAL_ERROR_SET_PARAMS_MSG "[cal] ERROR: failed to set parameters, sensor %u" +#define CAL_ERROR_SAVE_PARAMS_MSG "[cal] ERROR: failed to save parameters" #endif /* CALIBRATION_MESSAGES_H_ */ diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 192b8c727..a320c5c5b 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -48,6 +48,8 @@ #include #include +#include + #include "calibration_routines.h" #include "calibration_messages.h" #include "commander_helper.h" @@ -230,23 +232,19 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], return 0; } -enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub) +enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub, int accel_sub, bool lenient_still_position) { const unsigned ndim = 3; struct accel_report sensor; - /* exponential moving average of accel */ - float accel_ema[ndim] = { 0.0f }; - /* max-hold dispersion of accel */ - float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; - /* EMA time constant in seconds*/ - float ema_len = 0.5f; - /* set "still" threshold to 0.25 m/s^2 */ - float still_thr2 = powf(0.25f, 2); - /* set accel error threshold to 5m/s^2 */ - float accel_err_thr = 5.0f; - /* still time required in us */ - hrt_abstime still_time = 2000000; + float accel_ema[ndim] = { 0.0f }; // exponential moving average of accel + float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; // max-hold dispersion of accel + float ema_len = 0.5f; // EMA time constant in seconds + const float normal_still_thr = 0.25; // normal still threshold + float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2); + float accel_err_thr = 5.0f; // set accel error threshold to 5m/s^2 + hrt_abstime still_time = lenient_still_position ? 1000000 : 1500000; // still time required in us + struct pollfd fds[1]; fds[0].fd = accel_sub; fds[0].events = POLLIN; @@ -308,7 +306,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub) /* is still now */ if (t_still == 0) { /* first time */ - mavlink_and_console_log_info(mavlink_fd, "detected rest position, hold still..."); + mavlink_and_console_log_info(mavlink_fd, "[cal] detected rest position, hold still..."); t_still = t; t_timeout = t + timeout; @@ -325,7 +323,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub) accel_disp[2] > still_thr2 * 4.0f) { /* not still, reset still start time */ if (t_still != 0) { - mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still..."); + mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still..."); sleep(3); t_still = 0; } @@ -340,7 +338,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub) } if (poll_errcount > 1000) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG); return DETECT_ORIENTATION_ERROR; } } @@ -381,7 +379,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub) return DETECT_ORIENTATION_RIGHTSIDE_UP; // [ 0, 0, -g ] } - mavlink_and_console_log_critical(mavlink_fd, "ERROR: invalid orientation"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: invalid orientation"); return DETECT_ORIENTATION_ERROR; // Can't detect orientation } @@ -401,28 +399,35 @@ const char* detect_orientation_str(enum detect_orientation_return orientation) return rgOrientationStrs[orientation]; } -int calibrate_from_orientation(int mavlink_fd, - bool side_data_collected[detect_orientation_side_count], - calibration_from_orientation_worker_t calibration_worker, - void* worker_data) +calibrate_return calibrate_from_orientation(int mavlink_fd, + int cancel_sub, + bool side_data_collected[detect_orientation_side_count], + calibration_from_orientation_worker_t calibration_worker, + void* worker_data, + bool lenient_still_position) { - int result = OK; + calibrate_return result = calibrate_return_ok; // Setup subscriptions to onboard accel sensor int sub_accel = orb_subscribe_multi(ORB_ID(sensor_accel), 0); if (sub_accel < 0) { - mavlink_and_console_log_critical(mavlink_fd, "No onboard accel found, abort"); - return ERROR; + mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: No onboard accel found"); + return calibrate_return_error; } unsigned orientation_failures = 0; - // Rotate through all three main positions + // Rotate through all requested orientation while (true) { - if (orientation_failures > 10) { - result = ERROR; - mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_ORIENTATION_TIMEOUT); + if (calibrate_cancel_check(mavlink_fd, cancel_sub)) { + result = calibrate_return_cancelled; + break; + } + + if (orientation_failures > 4) { + result = calibrate_return_error; + mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: timeout waiting for orientation"); break; } @@ -450,32 +455,35 @@ int calibrate_from_orientation(int mavlink_fd, strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation)); } } - mavlink_and_console_log_info(mavlink_fd, "pending:%s", pendingStr); + mavlink_and_console_log_info(mavlink_fd, "[cal] pending:%s", pendingStr); - mavlink_and_console_log_info(mavlink_fd, "hold the vehicle still on one of the pending sides"); - enum detect_orientation_return orient = detect_orientation(mavlink_fd, sub_accel); + mavlink_and_console_log_info(mavlink_fd, "[cal] hold vehicle still on a pending side"); + enum detect_orientation_return orient = detect_orientation(mavlink_fd, cancel_sub, sub_accel, lenient_still_position); if (orient == DETECT_ORIENTATION_ERROR) { orientation_failures++; - mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still..."); + mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still..."); continue; } /* inform user about already handled side */ if (side_data_collected[orient]) { orientation_failures++; - mavlink_and_console_log_info(mavlink_fd, "%s side already completed or not needed", detect_orientation_str(orient)); - mavlink_and_console_log_info(mavlink_fd, "rotate to a pending side"); + mavlink_and_console_log_info(mavlink_fd, "[cal] %s side completed or not needed", detect_orientation_str(orient)); + mavlink_and_console_log_info(mavlink_fd, "[cal] rotate to a pending side"); continue; } - mavlink_and_console_log_info(mavlink_fd, "%s orientation detected", detect_orientation_str(orient)); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient)); orientation_failures = 0; // Call worker routine - calibration_worker(orient, worker_data); + result = calibration_worker(orient, cancel_sub, worker_data); + if (result != calibrate_return_ok ) { + break; + } - mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(orient)); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient)); // Note that this side is complete side_data_collected[orient] = true; @@ -487,7 +495,62 @@ int calibrate_from_orientation(int mavlink_fd, close(sub_accel); } - // FIXME: Do we need an orientation complete routine? - return result; } + +int calibrate_cancel_subscribe(void) +{ + return orb_subscribe(ORB_ID(vehicle_command)); +} + +void calibrate_cancel_unsubscribe(int cmd_sub) +{ + orb_unsubscribe(cmd_sub); +} + +static void calibrate_answer_command(int mavlink_fd, struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result) +{ + switch (result) { + case VEHICLE_CMD_RESULT_ACCEPTED: + tune_positive(true); + break; + + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(mavlink_fd, "command denied during calibration: %u", cmd.command); + tune_negative(true); + break; + + default: + break; + } +} + +bool calibrate_cancel_check(int mavlink_fd, int cancel_sub) +{ + struct pollfd fds[1]; + fds[0].fd = cancel_sub; + fds[0].events = POLLIN; + + if (poll(&fds[0], 1, 0) > 0) { + struct vehicle_command_s cmd; + memset(&cmd, 0, sizeof(cmd)); + + orb_copy(ORB_ID(vehicle_command), cancel_sub, &cmd); + + if (cmd.command == VEHICLE_CMD_PREFLIGHT_CALIBRATION && + (int)cmd.param1 == 0 && + (int)cmd.param2 == 0 && + (int)cmd.param3 == 0 && + (int)cmd.param4 == 0 && + (int)cmd.param5 == 0 && + (int)cmd.param6 == 0) { + calibrate_answer_command(mavlink_fd, cmd, VEHICLE_CMD_RESULT_ACCEPTED); + mavlink_log_critical(mavlink_fd, CAL_QGC_CANCELLED_MSG); + return true; + } else { + calibrate_answer_command(mavlink_fd, cmd, VEHICLE_CMD_RESULT_DENIED); + } + } + + return false; +} \ No newline at end of file diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index 8f34f0204..b8232730a 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2015 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 @@ -31,12 +31,8 @@ * ****************************************************************************/ -/** - * @file calibration_routines.h - * Calibration routines definitions. - * - * @author Lorenz Meier - */ +/// @file calibration_routines.h +/// @authot Don Gagne /** * Least-squares fit of a sphere to a set of points. @@ -75,30 +71,45 @@ enum detect_orientation_return { }; static const unsigned detect_orientation_side_count = 6; -/** - * Wait for vehicle to become still and detect it's orientation. - * - * @param mavlink_fd the MAVLink file descriptor to print output to - * @param accel_sub Subscription to onboard accel - * - * @return detect_orientation)_return according to orientation when vehicle is still and ready for measurements, - * DETECT_ORIENTATION_ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 - */ -enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub); +/// Wait for vehicle to become still and detect it's orientation +/// @return Returns detect_orientation_return according to orientation when vehicle +/// and ready for measurements +enum detect_orientation_return detect_orientation(int mavlink_fd, ///< Mavlink fd to write output to + int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe + int accel_sub, ///< Orb subcription to accel sensor + bool lenient_still_detection); ///< true: Use more lenient still position detection - -/** - * Returns the human readable string representation of the orientation - * - * @param orientation Orientation to return string for, "error" if buffer is too small - * - * @return str Returned orientation string - */ +/// Returns the human readable string representation of the orientation +/// @param orientation Orientation to return string for, "error" if buffer is too small const char* detect_orientation_str(enum detect_orientation_return orientation); -typedef int (*calibration_from_orientation_worker_t)(detect_orientation_return orientation, void* worker_data); +enum calibrate_return { + calibrate_return_ok, + calibrate_return_error, + calibrate_return_cancelled +}; + +typedef calibrate_return (*calibration_from_orientation_worker_t)(detect_orientation_return orientation, ///< Orientation which was detected + int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe + void* worker_data); ///< Opaque worker data + +/// Perform calibration sequence which require a rest orientation detection prior to calibration. +/// @return OK: Calibration succeeded, ERROR: Calibration failed +calibrate_return calibrate_from_orientation(int mavlink_fd, ///< Mavlink fd to write output to + int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe + bool side_data_collected[detect_orientation_side_count], ///< Sides for which data still needs calibration + calibration_from_orientation_worker_t calibration_worker, ///< Worker routine which performs the actual calibration + void* worker_data, ///< Opaque data passed to worker routine + bool lenient_still_detection); ///< true: Use more lenient still position detection + +/// Called at the beginning of calibration in order to subscribe to the cancel command +/// @return Handle to vehicle_command subscription +int calibrate_cancel_subscribe(void); + +/// Called to cancel the subscription to the cancel command +/// @param cancel_sub Cancel subcription from calibration_cancel_subscribe +void calibrate_cancel_unsubscribe(int cancel_sub); -int calibrate_from_orientation(int mavlink_fd, - bool side_data_collected[detect_orientation_side_count], - calibration_from_orientation_worker_t calibration_worker, - void* worker_data); +/// Used to periodically check for a cancel command +bool calibrate_cancel_check(int mavlink_fd, ///< Mavlink fd for output + int cancel_sub); ///< Cancel subcription fromcalibration_cancel_subscribe diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 57afcf19a..8cce01778 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2723,35 +2723,32 @@ void *commander_low_prio_loop(void *arg) /* enable RC control input */ status.rc_input_blocked = false; mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN"); + calib_ret = OK; } - - /* this always succeeds */ - calib_ret = OK; - } if (calib_ret == OK) { tune_positive(true); - } else { - tune_negative(true); - } + // Update preflight check status + // we do not set the calibration return value based on it because the calibration + // might have worked just fine, but the preflight check fails for a different reason, + // so this would be prone to false negatives. - // Update preflight check status - // we do not set the calibration return value based on it because the calibration - // might have worked just fine, but the preflight check fails for a different reason, - // so this would be prone to false negatives. + bool checkAirspeed = false; + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { + checkAirspeed = true; + } - bool checkAirspeed = false; - /* Perform airspeed check only if circuit breaker is not - * engaged and it's not a rotary wing */ - if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { - checkAirspeed = true; - } + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); + arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); - arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + } else { + tune_negative(true); + } break; } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 291ccfe80..e5df4175d 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -39,6 +39,7 @@ #include "gyro_calibration.h" #include "calibration_messages.h" +#include "calibration_routines.h" #include "commander_helper.h" #include @@ -62,142 +63,180 @@ static const int ERROR = -1; static const char *sensor_name = "gyro"; -int do_gyro_calibration(int mavlink_fd) -{ - const unsigned max_gyros = 3; - - int32_t device_id[3]; - mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "HOLD STILL"); - - /* wait for the user to respond */ - sleep(2); - - struct gyro_scale gyro_scale_zero = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - struct gyro_scale gyro_scale[max_gyros] = {}; +static const unsigned max_gyros = 3; - int res = OK; - - char str[30]; +/// Data passed to calibration worker routine +typedef struct { + int mavlink_fd; + int32_t device_id[max_gyros]; + int gyro_sensor_sub[max_gyros]; + struct gyro_scale gyro_scale[max_gyros]; + struct gyro_report gyro_report_0; +} gyro_worker_data_t; +static calibrate_return gyro_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) +{ + gyro_worker_data_t* worker_data = (gyro_worker_data_t*)(data); + unsigned calibration_counter[max_gyros] = { 0 }; + const unsigned calibration_count = 5000; + struct gyro_report gyro_report; + unsigned poll_errcount = 0; + + struct pollfd fds[max_gyros]; for (unsigned s = 0; s < max_gyros; s++) { - - /* ensure all scale fields are initialized tha same as the first struct */ - (void)memcpy(&gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale[0])); - - sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - /* reset all offsets to zero and all scales to one */ - int fd = open(str, 0); - - if (fd < 0) { - continue; + fds[s].fd = worker_data->gyro_sensor_sub[s]; + fds[s].events = POLLIN; + } + + memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0)); + + /* use first gyro to pace, but count correctly per-gyro for statistics */ + while (calibration_counter[0] < calibration_count) { + if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) { + return calibrate_return_cancelled; } - - device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); - - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); - close(fd); - - if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s); + + int poll_ret = poll(&fds[0], max_gyros, 1000); + + if (poll_ret > 0) { + + for (unsigned s = 0; s < max_gyros; s++) { + bool changed; + orb_check(worker_data->gyro_sensor_sub[s], &changed); + + if (changed) { + orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &gyro_report); + + if (s == 0) { + orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->gyro_report_0); + } + + worker_data->gyro_scale[s].x_offset += gyro_report.x; + worker_data->gyro_scale[s].y_offset += gyro_report.y; + worker_data->gyro_scale[s].z_offset += gyro_report.z; + calibration_counter[s]++; + } + + if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) { + mavlink_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count); + } + } + + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG); + return calibrate_return_error; } } - - unsigned calibration_counter[max_gyros] = { 0 }; - const unsigned calibration_count = 5000; - - struct gyro_report gyro_report_0 = {}; - - if (res == OK) { - /* determine gyro mean values */ - unsigned poll_errcount = 0; - - /* subscribe to gyro sensor topic */ - int sub_sensor_gyro[max_gyros]; - struct pollfd fds[max_gyros]; - - for (unsigned s = 0; s < max_gyros; s++) { - sub_sensor_gyro[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); - fds[s].fd = sub_sensor_gyro[s]; - fds[s].events = POLLIN; + + for (unsigned s = 0; s < max_gyros; s++) { + if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) { + mavlink_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s) + return calibrate_return_error; } - struct gyro_report gyro_report; - - /* use first gyro to pace, but count correctly per-gyro for statistics */ - while (calibration_counter[0] < calibration_count) { - /* wait blocking for new data */ - - int poll_ret = poll(&fds[0], max_gyros, 1000); - - if (poll_ret > 0) { - - for (unsigned s = 0; s < max_gyros; s++) { - bool changed; - orb_check(sub_sensor_gyro[s], &changed); - - if (changed) { - orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report); - - if (s == 0) { - orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report_0); - } + worker_data->gyro_scale[s].x_offset /= calibration_counter[s]; + worker_data->gyro_scale[s].y_offset /= calibration_counter[s]; + worker_data->gyro_scale[s].z_offset /= calibration_counter[s]; + } - gyro_scale[s].x_offset += gyro_report.x; - gyro_scale[s].y_offset += gyro_report.y; - gyro_scale[s].z_offset += gyro_report.z; - calibration_counter[s]++; - } + return calibrate_return_ok; +} - if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter[0] * 100) / calibration_count); - } - } +int do_gyro_calibration(int mavlink_fd) +{ + int res = OK; + gyro_worker_data_t worker_data; - } else { - poll_errcount++; - } + mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); - if (poll_errcount > 1000) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); - res = ERROR; - break; - } + worker_data.mavlink_fd = mavlink_fd; + + struct gyro_scale gyro_scale_zero = { + 0.0f, // x offset + 1.0f, // x scale + 0.0f, // y offset + 1.0f, // y scale + 0.0f, // z offset + 1.0f, // z scale + }; + + for (unsigned s = 0; s < max_gyros; s++) { + char str[30]; + + // Reset gyro ids to unavailable + worker_data.device_id[s] = 0; + (void)sprintf(str, "CAL_GYRO%u_ID", s); + res = param_set_no_notification(param_find(str), &(worker_data.device_id[s])); + if (res != OK) { + mavlink_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s); + return ERROR; } + + // Reset all offsets to 0 and scales to 1 + (void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale)); + sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); + int fd = open(str, 0); + if (fd >= 0) { + worker_data.device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); + close(fd); - for (unsigned s = 0; s < max_gyros; s++) { - close(sub_sensor_gyro[s]); - - gyro_scale[s].x_offset /= calibration_counter[s]; - gyro_scale[s].y_offset /= calibration_counter[s]; - gyro_scale[s].z_offset /= calibration_counter[s]; + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s); + return ERROR; + } } + + } + + for (unsigned s = 0; s < max_gyros; s++) { + worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); + } + + // Calibrate right-side up + + bool side_collected[detect_orientation_side_count] = { true, true, true, true, true, false }; + + int cancel_sub = calibrate_cancel_subscribe(); + calibrate_return cal_return = calibrate_from_orientation(mavlink_fd, // Mavlink fd to write output + cancel_sub, // Subscription to vehicle_command for cancel support + side_collected, // Sides to calibrate + gyro_calibration_worker, // Calibration worker + &worker_data, // Opaque data for calibration worked + true); // true: lenient still detection + calibrate_cancel_unsubscribe(cancel_sub); + + for (unsigned s = 0; s < max_gyros; s++) { + close(worker_data.gyro_sensor_sub[s]); } + if (cal_return == calibrate_return_cancelled) { + // Cancel message already sent, we are done here + return ERROR; + } else if (cal_return == calibrate_return_error) { + res = ERROR; + } + if (res == OK) { /* check offsets */ - float xdiff = gyro_report_0.x - gyro_scale[0].x_offset; - float ydiff = gyro_report_0.y - gyro_scale[0].y_offset; - float zdiff = gyro_report_0.z - gyro_scale[0].z_offset; + float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset; + float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset; + float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset; /* maximum allowable calibration error in radians */ const float maxoff = 0.0055f; - if (!isfinite(gyro_scale[0].x_offset) || - !isfinite(gyro_scale[0].y_offset) || - !isfinite(gyro_scale[0].z_offset) || + if (!isfinite(worker_data.gyro_scale[0].x_offset) || + !isfinite(worker_data.gyro_scale[0].y_offset) || + !isfinite(worker_data.gyro_scale[0].z_offset) || fabsf(xdiff) > maxoff || fabsf(ydiff) > maxoff || fabsf(zdiff) > maxoff) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: Motion during calibration"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration"); res = ERROR; } } @@ -207,40 +246,38 @@ int do_gyro_calibration(int mavlink_fd) bool failed = false; for (unsigned s = 0; s < max_gyros; s++) { + if (worker_data.device_id[s] != 0) { + char str[30]; + + (void)sprintf(str, "CAL_GYRO%u_XOFF", s); + failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].x_offset))); + (void)sprintf(str, "CAL_GYRO%u_YOFF", s); + failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].y_offset))); + (void)sprintf(str, "CAL_GYRO%u_ZOFF", s); + failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].z_offset))); + (void)sprintf(str, "CAL_GYRO%u_ID", s); + failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s]))); + + /* apply new scaling and offsets */ + (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); + int fd = open(str, 0); + + if (fd < 0) { + failed = true; + continue; + } - /* if any reasonable amount of data is missing, skip */ - if (calibration_counter[s] < calibration_count / 2) { - continue; - } - - (void)sprintf(str, "CAL_GYRO%u_XOFF", s); - failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].x_offset))); - (void)sprintf(str, "CAL_GYRO%u_YOFF", s); - failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].y_offset))); - (void)sprintf(str, "CAL_GYRO%u_ZOFF", s); - failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].z_offset))); - (void)sprintf(str, "CAL_GYRO%u_ID", s); - failed |= (OK != param_set_no_notification(param_find(str), &(device_id[s]))); - - /* apply new scaling and offsets */ - (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - int fd = open(str, 0); - - if (fd < 0) { - failed = true; - continue; - } + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[s]); + close(fd); - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]); - close(fd); - - if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG); + } } } if (failed) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to set offset params"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to set offset params"); res = ERROR; } } @@ -257,16 +294,14 @@ int do_gyro_calibration(int mavlink_fd) res = param_save_default(); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); + mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); } } - if (res == OK) { - mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - - } else { - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); - } + mavlink_log_info(mavlink_fd, res == OK ? CAL_QGC_DONE_MSG : CAL_QGC_FAILED_MSG, sensor_name); + // This give a chance for the log messages to go out of the queue before someone else stomps on then + sleep(1); + return res; } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 198acb027..3d10f98c1 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -65,8 +65,7 @@ static const int ERROR = -1; static const char *sensor_name = "mag"; static const unsigned max_mags = 3; -int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); -int mag_calibration_worker(detect_orientation_return orientation, void* worker_data); +calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); /// Data passed to calibration worker routine typedef struct { @@ -86,7 +85,7 @@ typedef struct { int do_mag_calibration(int mavlink_fd) { - mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); struct mag_scale mscale_null = { 0.0f, @@ -113,7 +112,7 @@ int do_mag_calibration(int mavlink_fd) (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); result = param_set_no_notification(param_find(str), &(device_ids[cur_mag])); if (result != OK) { - mavlink_and_console_log_info(mavlink_fd, "Unabled to reset CAL_MAG%u_ID", cur_mag); + mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag); break; } @@ -131,15 +130,15 @@ int do_mag_calibration(int mavlink_fd) result = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); if (result != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, cur_mag); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, cur_mag); } + /* calibrate range */ if (result == OK) { - /* calibrate range */ result = ioctl(fd, MAGIOCCALIBRATE, fd); if (result != OK) { - mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration, sensor %u", cur_mag); + mavlink_and_console_log_info(mavlink_fd, "[cal] Skipped scale calibration, sensor %u", cur_mag); /* this is non-fatal - mark it accordingly */ result = OK; } @@ -148,39 +147,48 @@ int do_mag_calibration(int mavlink_fd) close(fd); } + // Calibrate all mags at the same time if (result == OK) { - // Calibrate all mags at the same time - result = mag_calibrate_all(mavlink_fd, device_ids); - } - - if (result == OK) { - /* auto-save to EEPROM */ - result = param_save_default(); - if (result != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); + switch (mag_calibrate_all(mavlink_fd, device_ids)) { + case calibrate_return_cancelled: + // Cancel message already displayed, we're done here + result = ERROR; + break; + + case calibrate_return_ok: + /* auto-save to EEPROM */ + result = param_save_default(); + if (result == OK) { + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); + break; + } else { + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); + } + // Fall through + + default: + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); + break; } } - - if (result == OK) { - mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); - mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - } else { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); - } + // This give a chance for the log messages to go out of the queue before someone else stomps on then + sleep(1); + return result; } -int mag_calibration_worker(detect_orientation_return orientation, void* data) +static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) { - int result = OK; + calibrate_return result = calibrate_return_ok; unsigned int calibration_counter_side; mag_worker_data_t* worker_data = (mag_worker_data_t*)(data); - mavlink_and_console_log_info(worker_data->mavlink_fd, "Rotate vehicle around the detected orientation"); - mavlink_and_console_log_info(worker_data->mavlink_fd, "Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds); + mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Rotate vehicle around the detected orientation"); + mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds); sleep(2); uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; @@ -191,6 +199,11 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data) while (hrt_absolute_time() < calibration_deadline && calibration_counter_side < worker_data->calibration_points_perside) { + if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) { + result = calibrate_return_cancelled; + break; + } + // Wait clocking for new data on all mags struct pollfd fds[max_mags]; size_t fd_count = 0; @@ -222,8 +235,7 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data) // Progress indicator for side mavlink_and_console_log_info(worker_data->mavlink_fd, - "%s %s side calibration: progress <%u>", - sensor_name, + "[cal] %s side calibration: progress <%u>", detect_orientation_str(orientation), (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); } else { @@ -231,50 +243,25 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data) } if (poll_errcount > worker_data->calibration_points_perside * 3) { - result = ERROR; - mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_FAILED_SENSOR_MSG); + result = calibrate_return_error; + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG); break; } } - // Mark the opposite side as collected as well. No need to collect opposite side since it - // would generate similar points. - detect_orientation_return alternateOrientation = orientation; - switch (orientation) { - case DETECT_ORIENTATION_TAIL_DOWN: - alternateOrientation = DETECT_ORIENTATION_NOSE_DOWN; - break; - case DETECT_ORIENTATION_NOSE_DOWN: - alternateOrientation = DETECT_ORIENTATION_TAIL_DOWN; - break; - case DETECT_ORIENTATION_LEFT: - alternateOrientation = DETECT_ORIENTATION_RIGHT; - break; - case DETECT_ORIENTATION_RIGHT: - alternateOrientation = DETECT_ORIENTATION_LEFT; - break; - case DETECT_ORIENTATION_UPSIDE_DOWN: - alternateOrientation = DETECT_ORIENTATION_RIGHTSIDE_UP; - break; - case DETECT_ORIENTATION_RIGHTSIDE_UP: - alternateOrientation = DETECT_ORIENTATION_UPSIDE_DOWN; - break; - case DETECT_ORIENTATION_ERROR: - warnx("Invalid orientation in mag_calibration_worker"); - break; + if (result == calibrate_return_ok) { + mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation)); + + worker_data->done_count++; + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 34 * worker_data->done_count); } - worker_data->side_data_collected[alternateOrientation] = true; - mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(alternateOrientation)); - - worker_data->done_count++; - mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 34 * worker_data->done_count); return result; } -int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) +calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) { - int result = OK; + calibrate_return result = calibrate_return_ok; mag_worker_data_t worker_data; @@ -285,10 +272,13 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) worker_data.calibration_interval_perside_seconds = 20; worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; - // Initialize to collect all sides - for (size_t cur_side=0; cur_side<6; cur_side++) { - worker_data.side_data_collected[cur_side] = false; - } + // Collect: Right-side up, Left Side, Nose down + worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false; + worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false; + worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false; + worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true; + worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true; + worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true; for (size_t cur_mag=0; cur_mag(malloc(sizeof(float) * calibration_points_maxcount)); worker_data.z[cur_mag] = reinterpret_cast(malloc(sizeof(float) * calibration_points_maxcount)); if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory"); - result = ERROR; + mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: out of memory"); + result = calibrate_return_error; } } // Setup subscriptions to mag sensors - if (result == OK) { + if (result == calibrate_return_ok) { for (unsigned cur_mag=0; cur_mag Date: Sun, 26 Apr 2015 17:33:45 +0200 Subject: commander: Fix calibration feedback so that QGC picks up all error conditions --- src/modules/commander/accelerometer_calibration.cpp | 15 ++++++++++----- src/modules/commander/calibration_messages.h | 10 +++++----- src/modules/commander/calibration_routines.cpp | 8 ++++---- src/modules/commander/gyro_calibration.cpp | 13 ++++++++++--- src/modules/commander/mag_calibration.cpp | 8 ++++++-- 5 files changed, 35 insertions(+), 19 deletions(-) (limited to 'src') diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index d0982b341..f83640d28 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -222,8 +222,10 @@ int do_accel_calibration(int mavlink_fd) } } - if (res != OK || active_sensors == 0) { - mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG); + if (res != OK) { + if (active_sensors == 0) { + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG); + } return ERROR; } @@ -278,7 +280,7 @@ int do_accel_calibration(int mavlink_fd) fd = open(str, 0); if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "[cal] sensor does not exist"); + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "sensor does not exist"); res = ERROR; } else { res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); @@ -298,14 +300,17 @@ int do_accel_calibration(int mavlink_fd) mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); } + /* if there is a any preflight-check system response, let the barrage of messages through */ + usleep(200000); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); } else { mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); } - // This give a chance for the log messages to go out of the queue before someone else stomps on then - sleep(1); + /* give this message enough time to propagate */ + usleep(600000); return res; } diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index 2f6d02a72..7b4baae62 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -57,10 +57,10 @@ #define CAL_QGC_ORIENTATION_DETECTED_MSG "[cal] %s orientation detected" #define CAL_QGC_SIDE_DONE_MSG "[cal] %s side done, rotate to a different side" -#define CAL_ERROR_SENSOR_MSG "[cal] ERROR: failed reading sensor" -#define CAL_ERROR_RESET_CAL_MSG "[cal] ERROR: failed to reset calibration, sensor %u" -#define CAL_ERROR_APPLY_CAL_MSG "[cal] ERROR: failed to apply calibration, sensor %u" -#define CAL_ERROR_SET_PARAMS_MSG "[cal] ERROR: failed to set parameters, sensor %u" -#define CAL_ERROR_SAVE_PARAMS_MSG "[cal] ERROR: failed to save parameters" +#define CAL_ERROR_SENSOR_MSG "[cal] calibration failed: reading sensor" +#define CAL_ERROR_RESET_CAL_MSG "[cal] calibration failed: to reset, sensor %u" +#define CAL_ERROR_APPLY_CAL_MSG "[cal] calibration failed: to apply calibration, sensor %u" +#define CAL_ERROR_SET_PARAMS_MSG "[cal] calibration failed: to set parameters, sensor %u" +#define CAL_ERROR_SAVE_PARAMS_MSG "[cal] calibration failed: failed to save parameters" #endif /* CALIBRATION_MESSAGES_H_ */ diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index a320c5c5b..7e8c0fa52 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -324,7 +324,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub /* not still, reset still start time */ if (t_still != 0) { mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still..."); - sleep(3); + usleep(500000); t_still = 0; } } @@ -412,7 +412,7 @@ calibrate_return calibrate_from_orientation(int mavlink_fd, int sub_accel = orb_subscribe_multi(ORB_ID(sensor_accel), 0); if (sub_accel < 0) { - mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: No onboard accel found"); + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "No onboard accel"); return calibrate_return_error; } @@ -427,7 +427,7 @@ calibrate_return calibrate_from_orientation(int mavlink_fd, if (orientation_failures > 4) { result = calibrate_return_error; - mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: timeout waiting for orientation"); + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "timeout: no motion"); break; } @@ -488,7 +488,7 @@ calibrate_return calibrate_from_orientation(int mavlink_fd, // Note that this side is complete side_data_collected[orient] = true; tune_neutral(true); - sleep(1); + usleep(500000); } if (sub_accel >= 0) { diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index e5df4175d..bdef8771e 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -298,10 +298,17 @@ int do_gyro_calibration(int mavlink_fd) } } - mavlink_log_info(mavlink_fd, res == OK ? CAL_QGC_DONE_MSG : CAL_QGC_FAILED_MSG, sensor_name); + /* if there is a any preflight-check system response, let the barrage of messages through */ + usleep(200000); - // This give a chance for the log messages to go out of the queue before someone else stomps on then - sleep(1); + if (res == OK) { + mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); + } else { + mavlink_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); + } + + /* give this message enough time to propagate */ + usleep(600000); return res; } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 3d10f98c1..8a8d85818 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -158,6 +158,10 @@ int do_mag_calibration(int mavlink_fd) case calibrate_return_ok: /* auto-save to EEPROM */ result = param_save_default(); + + /* if there is a any preflight-check system response, let the barrage of messages through */ + usleep(200000); + if (result == OK) { mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); @@ -173,8 +177,8 @@ int do_mag_calibration(int mavlink_fd) } } - // This give a chance for the log messages to go out of the queue before someone else stomps on then - sleep(1); + /* give this message enough time to propagate */ + usleep(600000); return result; } -- cgit v1.2.3 From b07964660e8b959cad410064b1f74f839b2c268f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Apr 2015 17:39:00 +0200 Subject: commander: prune old code, do not run preflight checks when nothing relevant in the system is changing. --- src/modules/commander/commander.cpp | 36 +++----------------------- src/modules/commander/state_machine_helper.cpp | 4 ++- 2 files changed, 7 insertions(+), 33 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8cce01778..54de05192 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -182,21 +182,6 @@ static struct safety_s safety; static struct vehicle_control_mode_s control_mode; static struct offboard_control_mode_s offboard_control_mode; -/* tasks waiting for low prio thread */ -typedef enum { - LOW_PRIO_TASK_NONE = 0, - LOW_PRIO_TASK_PARAM_SAVE, - LOW_PRIO_TASK_PARAM_LOAD, - LOW_PRIO_TASK_GYRO_CALIBRATION, - LOW_PRIO_TASK_MAG_CALIBRATION, - LOW_PRIO_TASK_ALTITUDE_CALIBRATION, - LOW_PRIO_TASK_RC_CALIBRATION, - LOW_PRIO_TASK_ACCEL_CALIBRATION, - LOW_PRIO_TASK_AIRSPEED_CALIBRATION -} low_prio_task_t; - -static low_prio_task_t low_prio_task = LOW_PRIO_TASK_NONE; - /** * The daemon app only briefly exists to start * the background job. The stack size assigned in the @@ -552,8 +537,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } else { - //Refuse to arm if preflight checks have failed - if(!status.hil_state != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) { + // Refuse to arm if preflight checks have failed + if (!status.hil_state != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) { mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed."); cmd_result = VEHICLE_CMD_RESULT_DENIED; break; @@ -1616,8 +1601,7 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* If in INIT state, try to proceed to STANDBY state */ - if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { - /* TODO: check for sensors */ + if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); @@ -1625,8 +1609,6 @@ int commander_thread_main(int argc, char *argv[]) arming_state_changed = true; } - } else { - /* TODO: Add emergency stuff if sensors are lost */ } @@ -2674,7 +2656,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed, - true /* fRunPreArmChecks */, mavlink_fd)) { + false /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -2755,16 +2737,6 @@ void *commander_low_prio_loop(void *arg) case VEHICLE_CMD_PREFLIGHT_STORAGE: { - bool checkAirspeed = false; - /* Perform airspeed check only if circuit breaker is not - * engaged and it's not a rotary wing */ - if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { - checkAirspeed = true; - } - - // Update preflight check status - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); - if (((int)(cmd.param1)) == 0) { int ret = param_load_default(); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index bd31be7ac..844e8f2ab 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -213,7 +213,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Sensors need to be initialized for STANDBY state if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { - mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors not operational."); + mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); feedback_provided = true; valid_transition = false; status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; @@ -235,6 +235,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s status->condition_system_sensors_initialized) { mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, power cycle to complete"); feedback_provided = true; + } else { + } } -- cgit v1.2.3 From a57030c83685c64941977ecaaf865004cff51e9d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Apr 2015 18:27:48 +0200 Subject: commander: Do not lock down the system once HIL has kickeed in --- src/modules/commander/state_machine_helper.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 844e8f2ab..73fdb0940 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -140,6 +140,11 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s prearm_ret = OK; status->condition_system_sensors_initialized = true; + /* recover from a prearm fail */ + if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { + status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY; + } + } else { armed->lockdown = false; } @@ -211,8 +216,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s valid_transition = true; } - // Sensors need to be initialized for STANDBY state - if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + // Sensors need to be initialized for STANDBY state, except for HIL + if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) && + (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && + (!status->condition_system_sensors_initialized)) { mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); feedback_provided = true; valid_transition = false; -- cgit v1.2.3 From b739ad1a80bed898c8ff9466bbcb7e99c428d803 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Apr 2015 23:36:16 +0200 Subject: Remove reference to completely unused parameter --- src/modules/navigator/rtl_params.c | 12 ------------ 1 file changed, 12 deletions(-) (limited to 'src') diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 7800a6f03..3d8c78cf1 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -47,18 +47,6 @@ * RTL parameters, accessible via MAVLink */ -/** - * Loiter radius after RTL (FW only) - * - * Default value of loiter radius after RTL (fixedwing only). - * - * @unit meters - * @min 20 - * @max 200 - * @group Return To Land - */ -PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); - /** * RTL altitude * -- cgit v1.2.3 From 8e4c78cd2d74eb7cba5678772852a15f04b31b6e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Apr 2015 00:21:25 +0200 Subject: Load all GCS-required params --- src/modules/sensors/sensors.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 16e766646..aeb550fe9 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -624,13 +624,15 @@ Sensors::Sensors() : // These are parameters for which QGroundControl always expects to be returned in a list request. // We do a param_find here to force them into the list. - param_find("RC_CHAN_CNT"); - param_find("CAL_MAG0_ID"); - param_find("CAL_MAG1_ID"); - param_find("CAL_MAG2_ID"); - param_find("CAL_MAG0_ROT"); - param_find("CAL_MAG1_ROT"); - param_find("CAL_MAG2_ROT"); + (void)param_find("RC_CHAN_CNT"); + (void)param_find("RC_TH_USER"); + (void)param_find("CAL_MAG0_ID"); + (void)param_find("CAL_MAG1_ID"); + (void)param_find("CAL_MAG2_ID"); + (void)param_find("CAL_MAG0_ROT"); + (void)param_find("CAL_MAG1_ROT"); + (void)param_find("CAL_MAG2_ROT"); + (void)param_find("SYS_PARAM_VER"); /* fetch initial parameter values */ parameters_update(); -- cgit v1.2.3 From ed12d9c733e18d2955822a8e25e95377a62476a9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Apr 2015 09:03:35 +0200 Subject: systemlib: Fix param used counting --- src/modules/systemlib/param/param.c | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 5dfef9dce..4ec885ab3 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -287,7 +287,7 @@ param_for_used_index(unsigned index) /* we found the right used count, * return the param value */ - if (i == count) { + if (index == count) { return (param_t)i; } @@ -303,8 +303,9 @@ param_for_used_index(unsigned index) int param_get_index(param_t param) { - if (handle_in_range(param)) + if (handle_in_range(param)) { return (unsigned)param; + } return -1; } @@ -312,7 +313,9 @@ param_get_index(param_t param) int param_get_used_index(param_t param) { - if (!handle_in_range(param)) { + int param_storage_index = param_get_index(param); + + if (param_storage_index < 0) { return -1; } @@ -322,12 +325,17 @@ param_get_used_index(param_t param) for (unsigned i = 0; i < (unsigned)param + 1; i++) { for (unsigned j = 0; j < 8; j++) { if (param_changed_storage[i] & (1 << j)) { + + if (param_storage_index == i) { + return count; + } + count++; } } } - return count; + return -1; } const char * -- cgit v1.2.3 From 965e7cce037d7664df99af5ba9ae10b114a9b8dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Apr 2015 09:03:49 +0200 Subject: mavlink app: Robustify param handling --- src/modules/mavlink/mavlink_parameters.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 2a8b1fdec..9c8794017 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -201,7 +201,7 @@ MavlinkParametersManager::send(const hrt_abstime t) send_param(p); } - if (_send_all_index >= (int) param_count()) { + if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) { _send_all_index = -1; } } -- cgit v1.2.3 From 6755c0de012843245bb26b69a678d9f814dc7385 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Apr 2015 09:04:07 +0200 Subject: param cmd: Show used and normal list indices --- src/systemcmds/param/param.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 6b855cf58..98443e716 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -284,9 +284,9 @@ do_show_print(void *arg, param_t param) } } - printf("%c %c %s: ", (param_used(param) ? 'x' : ' '), + printf("%c %c %s [%d,%d] : ", (param_used(param) ? 'x' : ' '), param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), - param_name(param)); + param_name(param), param_get_used_index(param), param_get_index(param)); /* * This case can be expanded to handle printing common structure types. -- cgit v1.2.3 From 346798b129b9f13b511e04ff61679951625865a5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 09:01:53 +0200 Subject: blinkm: Let user know that we just did not find one and this is not an error --- src/drivers/blinkm/blinkm.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index d749a0d7b..cfa59eb3a 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -316,7 +316,6 @@ BlinkM::init() ret = I2C::init(); if (ret != OK) { - warnx("I2C init failed"); return ret; } @@ -970,7 +969,7 @@ blinkm_main(int argc, char *argv[]) if (OK != g_blinkm->init()) { delete g_blinkm; g_blinkm = nullptr; - errx(1, "init failed"); + errx(1, "no BlinkM found"); } exit(0); -- cgit v1.2.3 From 3c76006541a521f5ce0a3ba47c0a9949c7eb50c4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 09:02:08 +0200 Subject: Board drivers: Only print if init fails --- src/drivers/boards/px4fmu-v1/px4fmu_init.c | 7 ------- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 7 ------- 2 files changed, 14 deletions(-) (limited to 'src') diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c index 293021f8b..c7d4f1ce4 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -216,8 +216,6 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); - message("[boot] Successfully initialized SPI port 1\r\n"); - /* * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects. * Keep the SPI2 init optional and conditionally initialize the ADC pins @@ -243,7 +241,6 @@ __EXPORT int nsh_archinitialize(void) /* Get the SPI port for the microSD slot */ - message("[boot] Initializing SPI port 3\n"); spi3 = up_spiinitialize(3); if (!spi3) { @@ -252,8 +249,6 @@ __EXPORT int nsh_archinitialize(void) return -ENODEV; } - message("[boot] Successfully initialized SPI port 3\n"); - /* Now bind the SPI interface to the MMCSD driver */ result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3); @@ -263,7 +258,5 @@ __EXPORT int nsh_archinitialize(void) return -ENODEV; } - message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); - return OK; } diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 9b25c574a..266642cbb 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -296,8 +296,6 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); - message("[boot] Initialized SPI port 1 (SENSORS)\n"); - /* Get the SPI port for the FRAM */ spi2 = up_spiinitialize(2); @@ -317,8 +315,6 @@ __EXPORT int nsh_archinitialize(void) SPI_SETMODE(spi2, SPIDEV_MODE3); SPI_SELECT(spi2, SPIDEV_FLASH, false); - message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n"); - spi4 = up_spiinitialize(4); /* Default SPI4 to 1MHz and de-assert the known chip selects. */ @@ -328,8 +324,6 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false); SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false); - message("[boot] Initialized SPI port 4\n"); - #ifdef CONFIG_MMCSD /* First, get an instance of the SDIO interface */ @@ -350,7 +344,6 @@ __EXPORT int nsh_archinitialize(void) /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ sdio_mediachange(sdio, true); - message("[boot] Initialized SDIO\n"); #endif return OK; -- cgit v1.2.3 From 3835b7a6ec712a566b00039fb16f6c0f022ed263 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 09:02:34 +0200 Subject: HMC5883: Let user know we just did not find one --- src/drivers/hmc5883/hmc5883.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 3a3848446..5bf88da3d 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1490,8 +1490,9 @@ start(enum HMC5883_BUS busid, enum Rotation rotation) started |= start_bus(bus_options[i], rotation); } - if (!started) - errx(1, "driver start failed"); + if (!started) { + exit(1); + } } /** -- cgit v1.2.3 From ebaac07ab2662c9ae443a69d8e0fd12f07b6f5e6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 09:02:50 +0200 Subject: PX4 flow driver: Let user know we just did not find one --- src/drivers/px4flow/px4flow.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 8ea7a6170..b343a3e30 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -674,7 +674,7 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + errx(1, "no PX4 FLOW connected"); } /** -- cgit v1.2.3 From d3261069806e3ecb9627f273fcb58c68e5523ac7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 09:03:08 +0200 Subject: RGB led: Let user know we just did not find one --- src/drivers/rgbled/rgbled.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 1e38a766e..512d6318d 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -625,7 +625,7 @@ rgbled_main(int argc, char *argv[]) if (g_rgbled == nullptr) { // fall back to default bus if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { - errx(1, "init failed"); + errx(1, "no RGB led on bus #%d", i2cdevice); } i2cdevice = PX4_I2C_BUS_LED; } @@ -640,7 +640,7 @@ rgbled_main(int argc, char *argv[]) if (OK != g_rgbled->init()) { delete g_rgbled; g_rgbled = nullptr; - errx(1, "init failed"); + errx(1, "no RGB led on bus #%d", i2cdevice); } } -- cgit v1.2.3 From 3f8c81433e4cbc6726303c5b4bde28a97b32668e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 09:03:48 +0200 Subject: commander: Provide more useful mission feedback --- src/modules/commander/commander.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 54de05192..5eb0ab6ec 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -945,10 +945,10 @@ int commander_thread_main(int argc, char *argv[]) if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { - warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, - mission.current_seq); - mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d", - mission.dataman_id, mission.count, mission.current_seq); + if (mission.count > 0) { + mavlink_log_info(mavlink_fd, "[cmd] Mission #%d loaded, %u WPs, curr: %d", + mission.dataman_id, mission.count, mission.current_seq); + } } else { const char *missionfail = "reading mission state failed"; -- cgit v1.2.3 From 16b033982cacfc5bb1c60f641be29caafe7490aa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 09:04:11 +0200 Subject: Dataman: Be more compact in boot output --- src/modules/dataman/dataman.c | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 68bf12024..b442b7430 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -685,20 +685,21 @@ task_main(int argc, char *argv[]) fsync(g_task_fd); + printf("dataman: "); /* see if we need to erase any items based on restart type */ int sys_restart_val; if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) { if (sys_restart_val == DM_INIT_REASON_POWER_ON) { - warnx("Power on restart"); + printf("Power on restart"); _restart(DM_INIT_REASON_POWER_ON); } else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { - warnx("In flight restart"); + printf("In flight restart"); _restart(DM_INIT_REASON_IN_FLIGHT); } else { - warnx("Unknown restart"); + printf("Unknown restart"); } } else { - warnx("Unknown restart"); + printf("Unknown restart"); } /* We use two file descriptors, one for the caller context and one for the worker thread */ @@ -706,7 +707,7 @@ task_main(int argc, char *argv[]) /* worker thread is shutting down but still processing requests */ g_fd = g_task_fd; - warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset); + printf(", data manager file '%s' size is %d bytes\n", k_data_manager_device_path, max_offset); /* Tell startup that the worker thread has completed its initialization */ sem_post(&g_init_sema); -- cgit v1.2.3 From 1420a0c74c6d4acf1d2593e41d3bd6ad984c9985 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 09:04:28 +0200 Subject: Sensors: Be less verbose --- src/modules/sensors/sensors.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index aeb550fe9..4fbc0416e 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1623,7 +1623,8 @@ Sensors::parameter_update_poll(bool forced) close(fd); } - warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count); + /* do not output this for now, as its covered in preflight checks */ + // warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count); } } -- cgit v1.2.3 From 868b9b33ed6d7f521d20d28429cd8c7d57311409 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 26 Apr 2015 23:31:25 +0200 Subject: Make sure we log the airspeed check to the console as well. --- src/modules/commander/PreflightCheck.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index b0a587762..4d9bd8ae0 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -254,13 +254,13 @@ static bool airspeedCheck(int mavlink_fd, bool optional) if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) { - mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); success = false; goto out; } if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) { - mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); + mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); // XXX do not make this fatal yet } -- cgit v1.2.3 From ef63babb7137b1a0c2cdfed8af5881e0268b26b2 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Mon, 27 Apr 2015 13:29:36 +0200 Subject: Make sure circuit breakers are ready before the first preflight check call. --- src/modules/commander/commander.cpp | 30 ++++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5eb0ab6ec..7aaf5e5cd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -213,6 +213,8 @@ int commander_thread_main(int argc, char *argv[]); void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed); +void get_circuit_breaker_params(); + void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man); @@ -1114,6 +1116,8 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_sys_type, &(status.system_type)); // get system type status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status); + get_circuit_breaker_params(); + bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ @@ -1121,9 +1125,9 @@ int commander_thread_main(int argc, char *argv[]) checkAirspeed = true; } - //Run preflight check + // Run preflight check status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); - if(!status.condition_system_sensors_initialized) { + if (!status.condition_system_sensors_initialized) { set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune } else { @@ -1207,14 +1211,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); - status.circuit_breaker_engaged_power_check = - circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); - status.circuit_breaker_engaged_airspd_check = - circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); - status.circuit_breaker_engaged_enginefailure_check = - circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY); - status.circuit_breaker_engaged_gpsfailure_check = - circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY); + get_circuit_breaker_params(); status_changed = true; @@ -2107,6 +2104,19 @@ int commander_thread_main(int argc, char *argv[]) return 0; } +void +get_circuit_breaker_params() +{ + status.circuit_breaker_engaged_power_check = + circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status.circuit_breaker_engaged_airspd_check = + circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); + status.circuit_breaker_engaged_enginefailure_check = + circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY); + status.circuit_breaker_engaged_gpsfailure_check = + circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY); +} + void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed) { -- cgit v1.2.3 From bd1c3363df44170523c68fd87ee19f2582a5e8fc Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 27 Apr 2015 14:25:26 +0200 Subject: added new vtol mav types --- msg/vehicle_status.msg | 6 ++++-- src/modules/commander/commander_helper.cpp | 6 ++++-- 2 files changed, 8 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 2e001cac7..2a7e493a3 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -72,8 +72,10 @@ uint8 VEHICLE_TYPE_FLAPPING_WING = 16 # Flapping wing uint8 VEHICLE_TYPE_KITE = 17 # Kite uint8 VEHICLE_TYPE_ONBOARD_CONTROLLER=18 # Onboard companion controller uint8 VEHICLE_TYPE_VTOL_DUOROTOR = 19 # Vtol with two engines -uint8 VEHICLE_TYPE_VTOL_QUADROTOR = 20 # Vtol with four engines*/ -uint8 VEHICLE_TYPE_ENUM_END = 21 +uint8 VEHICLE_TYPE_VTOL_QUADROTOR = 20 # Vtol with four engines +uint8 VEHICLE_TYPE_VTOL_HEXAROTOR = 21 # Vtol with six engines +uint8 VEHICLE_TYPE_VTOL_OCTOROTOR = 22 # Vtol with eight engines +uint8 VEHICLE_TYPE_ENUM_END = 23 uint8 VEHICLE_BATTERY_WARNING_NONE = 0 # no battery low voltage warning active uint8 VEHICLE_BATTERY_WARNING_LOW = 1 # warning of low voltage diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 5f735b7b7..99b926be4 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -85,8 +85,10 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status) } bool is_vtol(const struct vehicle_status_s * current_status) { - return current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR || - current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR; + return (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR || + current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR || + current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_HEXAROTOR || + current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_OCTOROTOR); } static int buzzer = -1; -- cgit v1.2.3