From aa4bdd2af3daa015ef9def15a017dd85bad95df9 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 14 May 2015 09:57:19 -0600 Subject: [PATCH 01/84] change fmuv1 default config to INAV and att_estimator_q --- makefiles/config_px4fmu-v1_default.mk | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 55b73ffde3..014b0a8505 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -67,11 +67,10 @@ MODULES += modules/land_detector # Estimation modules (EKF / other filters) # # Too high RAM usage due to static allocations -# MODULES += modules/attitude_estimator_ekf -MODULES += modules/ekf_att_pos_estimator -# Since attitude_estimator_ekf is disabled, this app won't be -# worthwhile on its own -# MODULES += modules/position_estimator_inav +MODULES += modules/attitude_estimator_ekf +# MODULES += modules/ekf_att_pos_estimator +MODULES += modules/attitude_estimator_q +MODULES += modules/position_estimator_inav # # Vehicle Control From 71e40c2e16195b931fe4a653c6ab3de269561693 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 21 May 2015 10:27:46 -0600 Subject: [PATCH 02/84] compile in ekf_att_pos and Q att estimators --- makefiles/config_px4fmu-v1_default.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 014b0a8505..2e4829958e 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -67,8 +67,8 @@ MODULES += modules/land_detector # Estimation modules (EKF / other filters) # # Too high RAM usage due to static allocations -MODULES += modules/attitude_estimator_ekf -# MODULES += modules/ekf_att_pos_estimator +#MODULES += modules/attitude_estimator_ekf +MODULES += modules/ekf_att_pos_estimator MODULES += modules/attitude_estimator_q MODULES += modules/position_estimator_inav From 2142fcddd3e1e97e1cfca0698845646a570ef7d4 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 21 May 2015 12:41:50 -0600 Subject: [PATCH 03/84] add comment that EKF_att_estimator does not run on FMUv1 --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index beb9891d2e..914807889f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -13,6 +13,8 @@ then # and uses the older EKF filter. However users can # enable the new quaternion based complimentary # filter by setting EKF_ATT_ENABLED = 0. + # Note that on FMUv1, the EKF att estimator is not + # available and the Q estimator runs instead. if param compare EKF_ATT_ENABLED 1 then attitude_estimator_ekf start From b54d4f5b0536e70ac0f92b8aba499babf7a7f753 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 May 2015 13:15:03 +0200 Subject: [PATCH 04/84] vehicle status: Add field to disable RC input (and required checks for it) --- msg/vehicle_status.msg | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index c5d5ee9a16..1e17b72c67 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -121,7 +121,8 @@ bool rc_signal_found_once bool rc_signal_lost # true if RC reception lost uint64 rc_signal_lost_timestamp # Time at which the RC reception was lost bool rc_signal_lost_cmd # true if RC lost mode is commanded -bool rc_input_blocked # set if RC input should be ignored +bool rc_input_blocked # set if RC input should be ignored temporarily +bool rc_input_off # set if RC input should be disabled completely bool data_link_lost # datalink to GCS lost bool data_link_lost_cmd # datalink to GCS lost mode commanded From bed746c213c71670e6012c801e5c4588455311d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 May 2015 13:15:28 +0200 Subject: [PATCH 05/84] commander: Add param and mode to disable RC input in general and required validation / setup. --- src/modules/commander/commander.cpp | 13 +++++++++---- src/modules/commander/commander_params.c | 12 ++++++++++++ src/modules/commander/state_machine_helper.cpp | 2 +- 3 files changed, 22 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a9b2926d2a..5bbcf226a2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -828,6 +828,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_ef_time_thres = param_find("COM_EF_TIME"); param_t _param_autostart_id = param_find("SYS_AUTOSTART"); param_t _param_autosave_params = param_find("COM_AUTOS_PAR"); + param_t _param_rc_in_off = param_find("COM_RC_IN_OFF"); const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; @@ -888,6 +889,7 @@ int commander_thread_main(int argc, char *argv[]) status.condition_landed = true; // initialize to safe value // We want to accept RC inputs as default status.rc_input_blocked = false; + status.rc_input_off = false; status.main_state =vehicle_status_s::MAIN_STATE_MANUAL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status.arming_state = vehicle_status_s::ARMING_STATE_INIT; @@ -1132,7 +1134,7 @@ int commander_thread_main(int argc, char *argv[]) } // Run preflight check - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check); + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_off, !status.circuit_breaker_engaged_gpsfailure_check); if (!status.condition_system_sensors_initialized) { set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune } @@ -1147,6 +1149,7 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; float rc_loss_timeout = 0.5; + int32_t rc_in_off = 0; int32_t datalink_regain_timeout = 0; /* Thresholds for engine failure detection */ @@ -1233,6 +1236,8 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_rc_loss_timeout, &rc_loss_timeout); + param_get(_param_rc_in_off, &rc_in_off); + status.rc_input_off = rc_in_off; param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); param_get(_param_ef_throttle_thres, &ef_throttle_thres); param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); @@ -1305,7 +1310,7 @@ int commander_thread_main(int argc, char *argv[]) } /* provide RC and sensor status feedback to the user */ - (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check); + (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, !status.rc_input_off, !status.circuit_breaker_engaged_gpsfailure_check); } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; @@ -1707,7 +1712,7 @@ int commander_thread_main(int argc, char *argv[]) } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination /* RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && + if (!status.rc_input_off && !status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { @@ -2770,7 +2775,7 @@ void *commander_low_prio_loop(void *arg) checkAirspeed = true; } - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check); + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_off, !status.circuit_breaker_engaged_gpsfailure_check); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 5f43fd77cd..2a4c91c554 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -195,3 +195,15 @@ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5); * @max 1 */ PARAM_DEFINE_INT32(COM_AUTOS_PAR, 1); + +/** + * Disable RC control input + * + * Setting this to 1 disables RC input handling and the associated checks. This is + * mainly inteded for pure offboard control or tablet control use. + * + * @group Commander + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(COM_RC_IN_OFF, 0); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index b53eeebeb6..045fbd5fd2 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -692,5 +692,5 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) checkAirspeed = true; } - return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status->circuit_breaker_engaged_gpsfailure_check, true); + return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status->rc_input_off, !status->circuit_breaker_engaged_gpsfailure_check, true); } From af546def913c7f4d6f1257267ecb8556aba6a277 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 29 Apr 2015 15:54:21 +0200 Subject: [PATCH 06/84] mavlink: use 'buttons' field of MANUAL_CONTROL message as mode switches --- src/modules/mavlink/mavlink_receiver.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 29b4fbcabb..8f31b2c858 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -899,12 +899,21 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) } } +static switch_pos_t decode_switch_pos(uint16_t buttons, int sw) { + return (buttons >> (sw * 2)) & 3; +} + void MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) { mavlink_manual_control_t man; mavlink_msg_manual_control_decode(msg, &man); + // Check target + if (man.target != 0 && man.target != _mavlink->get_system_id()) { + return; + } + struct manual_control_setpoint_s manual; memset(&manual, 0, sizeof(manual)); @@ -914,6 +923,13 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.r = man.r / 1000.0f; manual.z = man.z / 1000.0f; + manual.mode_switch = decode_switch_pos(man.buttons, 0); + manual.return_switch = decode_switch_pos(man.buttons, 1); + manual.posctl_switch = decode_switch_pos(man.buttons, 2); + manual.loiter_switch = decode_switch_pos(man.buttons, 3); + manual.acro_switch = decode_switch_pos(man.buttons, 4); + manual.offboard_switch = decode_switch_pos(man.buttons, 5); + // warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z); if (_manual_pub < 0) { From 01fd84e4dc0aaa1fbf82f7355cca241a76ece602 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 May 2015 17:40:06 +0200 Subject: [PATCH 07/84] mavlink and commander app: Add support for manual input, either directly or via remote control --- msg/vehicle_status.msg | 4 ++ src/drivers/drv_rc_input.h | 3 +- src/modules/commander/commander.cpp | 4 +- src/modules/commander/commander_params.c | 10 +-- src/modules/mavlink/mavlink_main.cpp | 3 + src/modules/mavlink/mavlink_main.h | 18 +++++ src/modules/mavlink/mavlink_receiver.cpp | 68 ++++++++++++++----- .../navigator/mission_feasibility_checker.cpp | 6 +- 8 files changed, 88 insertions(+), 28 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 1e17b72c67..4d9716f6c9 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -81,6 +81,10 @@ uint8 VEHICLE_BATTERY_WARNING_NONE = 0 # no battery low voltage warning acti uint8 VEHICLE_BATTERY_WARNING_LOW = 1 # warning of low voltage uint8 VEHICLE_BATTERY_WARNING_CRITICAL = 2 # alerting of critical voltage +uint8 RC_IN_MODE_DEFAULT = 0 +uint8 RC_IN_MODE_OFF = 1 +uint8 RC_IN_MODE_GENERATED = 2 + # state machine / state of vehicle. # Encodes the complete system state and is set by the commander app. uint16 counter # incremented by the writing thread everytime new data is stored diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index a8a76c3eff..fce5249b5e 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -99,7 +99,8 @@ enum RC_INPUT_SOURCE { RC_INPUT_SOURCE_PX4IO_PPM, RC_INPUT_SOURCE_PX4IO_SPEKTRUM, RC_INPUT_SOURCE_PX4IO_SBUS, - RC_INPUT_SOURCE_PX4IO_ST24 + RC_INPUT_SOURCE_PX4IO_ST24, + RC_INPUT_SOURCE_MAVLINK }; /** diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5bbcf226a2..2e68de08d2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -828,7 +828,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_ef_time_thres = param_find("COM_EF_TIME"); param_t _param_autostart_id = param_find("SYS_AUTOSTART"); param_t _param_autosave_params = param_find("COM_AUTOS_PAR"); - param_t _param_rc_in_off = param_find("COM_RC_IN_OFF"); + param_t _param_rc_in_off = param_find("COM_RC_IN_MODE"); const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; @@ -1237,7 +1237,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_rc_loss_timeout, &rc_loss_timeout); param_get(_param_rc_in_off, &rc_in_off); - status.rc_input_off = rc_in_off; + status.rc_input_off = (rc_in_off == vehicle_status_s::RC_IN_MODE_OFF); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); param_get(_param_ef_throttle_thres, &ef_throttle_thres); param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 2a4c91c554..2225b00d35 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -199,11 +199,13 @@ PARAM_DEFINE_INT32(COM_AUTOS_PAR, 1); /** * Disable RC control input * - * Setting this to 1 disables RC input handling and the associated checks. This is - * mainly inteded for pure offboard control or tablet control use. + * The default value of 0 requires a valid RC transmitter setup. + * Setting this to 1 disables RC input handling and the associated checks. A value of + * 2 will generate RC control data from manual input received via MAVLink instead + * of directly forwarding the manual input data. * * @group Commander * @min 0 - * @max 1 + * @max 2 */ -PARAM_DEFINE_INT32(COM_RC_IN_OFF, 0); +PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index be63ae861c..9be21b25d7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -123,6 +123,7 @@ Mavlink::Mavlink() : _mavlink_fd(-1), _task_running(false), _hil_enabled(false), + _generate_rc(false), _use_hil_gps(false), _forward_externalsp(false), _is_usb_uart(false), @@ -1497,6 +1498,8 @@ Mavlink::task_main(int argc, char *argv[]) if (status_sub->update(&status_time, &status)) { /* switch HIL mode if required */ set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON); + + set_manual_input_mode_generation(status.rc_input_off == vehicle_status_s::RC_IN_MODE_GENERATED); } /* check for requested subscriptions */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index b732355b44..71749e2923 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -170,6 +170,23 @@ public: */ int set_hil_enabled(bool hil_enabled); + /** + * Set manual input generation mode + * + * Set to true to generate RC_INPUT messages on the system bus from + * MAVLink messages. + * + * @param generation_enabled If set to true, generate RC_INPUT messages + */ + void set_manual_input_mode_generation(bool generation_enabled) { _generate_rc = generation_enabled; } + + /** + * Get the manual input generation mode + * + * @return true if manual inputs should generate RC data + */ + bool get_manual_input_mode_generation() { return _generate_rc; } + void send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0); /** @@ -287,6 +304,7 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ + bool _generate_rc; /**< Generate RC messages from manual input MAVLink messages */ bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */ bool _is_usb_uart; /**< Port is USB */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8f31b2c858..8801a92aa2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -56,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -914,29 +915,60 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) return; } - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); + if (_mavlink->get_manual_input_mode_generation()) { + + struct rc_input_values rc = {}; + rc.timestamp_publication = hrt_absolute_time(); + rc.timestamp_last_signal = rc.timestamp_publication; + + rc.channel_count = 8; + rc.rc_failsafe = false; + rc.rc_lost = false; + rc.rc_lost_frame_count = 0; + rc.rc_total_frame_count = 1; + rc.rc_ppm_frame_length = (2 * rc.channel_count + 4) * 1000; + rc.input_source = RC_INPUT_SOURCE_MAVLINK; + rc.values[0] = man.x * 500 + 1500; + rc.values[1] = man.y * 500 + 1500; + rc.values[2] = man.r / 2.0f + 1500; + rc.values[3] = man.z / 2.0f + 1500; + + rc.values[4] = decode_switch_pos(man.buttons, 0) * 1000 + 1000; + rc.values[5] = decode_switch_pos(man.buttons, 1) * 1000 + 1000; + rc.values[6] = decode_switch_pos(man.buttons, 2) * 1000 + 1000; + rc.values[7] = decode_switch_pos(man.buttons, 3) * 1000 + 1000; + rc.values[8] = decode_switch_pos(man.buttons, 4) * 1000 + 1000; + rc.values[9] = decode_switch_pos(man.buttons, 5) * 1000 + 1000; + + if (_rc_pub <= 0) { + _rc_pub = orb_advertise(ORB_ID(input_rc), &rc); - manual.timestamp = hrt_absolute_time(); - manual.x = man.x / 1000.0f; - manual.y = man.y / 1000.0f; - manual.r = man.r / 1000.0f; - manual.z = man.z / 1000.0f; + } else { + orb_publish(ORB_ID(input_rc), _rc_pub, &rc); + } - manual.mode_switch = decode_switch_pos(man.buttons, 0); - manual.return_switch = decode_switch_pos(man.buttons, 1); - manual.posctl_switch = decode_switch_pos(man.buttons, 2); - manual.loiter_switch = decode_switch_pos(man.buttons, 3); - manual.acro_switch = decode_switch_pos(man.buttons, 4); - manual.offboard_switch = decode_switch_pos(man.buttons, 5); + } else { + struct manual_control_setpoint_s manual = {}; - // warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z); + manual.timestamp = hrt_absolute_time(); + manual.x = man.x / 1000.0f; + manual.y = man.y / 1000.0f; + manual.r = man.r / 1000.0f; + manual.z = man.z / 1000.0f; - if (_manual_pub < 0) { - _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); + manual.mode_switch = decode_switch_pos(man.buttons, 0); + manual.return_switch = decode_switch_pos(man.buttons, 1); + manual.posctl_switch = decode_switch_pos(man.buttons, 2); + manual.loiter_switch = decode_switch_pos(man.buttons, 3); + manual.acro_switch = decode_switch_pos(man.buttons, 4); + manual.offboard_switch = decode_switch_pos(man.buttons, 5); - } else { - orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); + if (_manual_pub < 0) { + _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); + + } else { + orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); + } } } diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 8f1d6f8e85..d9297f25bf 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -127,7 +127,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss } if (!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) { - mavlink_log_critical(_mavlink_fd, "Geofence violation waypoint %d", i); + mavlink_log_critical(_mavlink_fd, "Geofence violation for waypoint %d", i); return false; } } @@ -177,7 +177,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s if (dm_read(dm_current, i, &missionitem, len) != len) { // not supposed to happen unless the datamanager can't access the SD card, etc. - mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Cannot access mission from SD card"); + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Cannot access SD card"); return false; } @@ -197,7 +197,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s return false; } } - mavlink_log_critical(_mavlink_fd, "Mission is valid!"); + mavlink_log_info(_mavlink_fd, "Mission ready."); return true; } From 2bb655c46c7995c53672a9ebd7567af8db86c49f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 May 2015 18:21:28 +0200 Subject: [PATCH 08/84] mavlink app: Improved mapping to RC input --- src/drivers/drv_rc_input.h | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index fce5249b5e..b82ced3845 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -119,7 +119,7 @@ struct rc_input_values { /** number of channels actually being seen */ uint32_t channel_count; - /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */ + /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception */ int32_t rssi; /** diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8801a92aa2..04b368a434 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -926,8 +926,9 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.rc_lost = false; rc.rc_lost_frame_count = 0; rc.rc_total_frame_count = 1; - rc.rc_ppm_frame_length = (2 * rc.channel_count + 4) * 1000; + rc.rc_ppm_frame_length = 0; rc.input_source = RC_INPUT_SOURCE_MAVLINK; + rc.rssi = RC_INPUT_RSSI_MAX; rc.values[0] = man.x * 500 + 1500; rc.values[1] = man.y * 500 + 1500; rc.values[2] = man.r / 2.0f + 1500; From 009815deb0ee14e74f1bb6ebcce0f028866e044f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 May 2015 20:10:35 +0200 Subject: [PATCH 09/84] Improve config and mapping --- msg/vehicle_status.msg | 2 +- src/modules/commander/commander.cpp | 12 ++++---- .../commander/state_machine_helper.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 30 ++++++++++++------- 4 files changed, 28 insertions(+), 18 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 4d9716f6c9..3fe2426593 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -126,7 +126,7 @@ bool rc_signal_lost # true if RC reception lost uint64 rc_signal_lost_timestamp # Time at which the RC reception was lost bool rc_signal_lost_cmd # true if RC lost mode is commanded bool rc_input_blocked # set if RC input should be ignored temporarily -bool rc_input_off # set if RC input should be disabled completely +uint8 rc_input_off # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping. bool data_link_lost # datalink to GCS lost bool data_link_lost_cmd # datalink to GCS lost mode commanded diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2e68de08d2..5f022370a0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -889,7 +889,7 @@ int commander_thread_main(int argc, char *argv[]) status.condition_landed = true; // initialize to safe value // We want to accept RC inputs as default status.rc_input_blocked = false; - status.rc_input_off = false; + status.rc_input_off = vehicle_status_s::RC_IN_MODE_DEFAULT; status.main_state =vehicle_status_s::MAIN_STATE_MANUAL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status.arming_state = vehicle_status_s::ARMING_STATE_INIT; @@ -1237,7 +1237,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_rc_loss_timeout, &rc_loss_timeout); param_get(_param_rc_in_off, &rc_in_off); - status.rc_input_off = (rc_in_off == vehicle_status_s::RC_IN_MODE_OFF); + status.rc_input_off = rc_in_off; param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); param_get(_param_ef_throttle_thres, &ef_throttle_thres); param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); @@ -1310,7 +1310,8 @@ int commander_thread_main(int argc, char *argv[]) } /* provide RC and sensor status feedback to the user */ - (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, !status.rc_input_off, !status.circuit_breaker_engaged_gpsfailure_check); + (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, + !(status.rc_input_off == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; @@ -1712,7 +1713,7 @@ int commander_thread_main(int argc, char *argv[]) } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination /* RC input check */ - if (!status.rc_input_off && !status.rc_input_blocked && sp_man.timestamp != 0 && + if (!(status.rc_input_off == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { @@ -2775,7 +2776,8 @@ void *commander_low_prio_loop(void *arg) checkAirspeed = true; } - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_off, !status.circuit_breaker_engaged_gpsfailure_check); + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, + !(status.rc_input_off == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 045fbd5fd2..0dedd22282 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -692,5 +692,5 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) checkAirspeed = true; } - return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status->rc_input_off, !status->circuit_breaker_engaged_gpsfailure_check, true); + return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_off == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true); } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 04b368a434..e0ce617fae 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -904,6 +904,14 @@ static switch_pos_t decode_switch_pos(uint16_t buttons, int sw) { return (buttons >> (sw * 2)) & 3; } +static int decode_switch_pos_n(uint16_t buttons, int sw) { + if (buttons & (1 << sw)) { + return 1; + } else { + return 0; + } +} + void MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) { @@ -929,17 +937,17 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.rc_ppm_frame_length = 0; rc.input_source = RC_INPUT_SOURCE_MAVLINK; rc.rssi = RC_INPUT_RSSI_MAX; - rc.values[0] = man.x * 500 + 1500; - rc.values[1] = man.y * 500 + 1500; - rc.values[2] = man.r / 2.0f + 1500; - rc.values[3] = man.z / 2.0f + 1500; - - rc.values[4] = decode_switch_pos(man.buttons, 0) * 1000 + 1000; - rc.values[5] = decode_switch_pos(man.buttons, 1) * 1000 + 1000; - rc.values[6] = decode_switch_pos(man.buttons, 2) * 1000 + 1000; - rc.values[7] = decode_switch_pos(man.buttons, 3) * 1000 + 1000; - rc.values[8] = decode_switch_pos(man.buttons, 4) * 1000 + 1000; - rc.values[9] = decode_switch_pos(man.buttons, 5) * 1000 + 1000; + rc.values[0] = man.x / 2 + 1500; + rc.values[1] = man.y / 2 + 1500; + rc.values[2] = man.r / 2 + 1500; + rc.values[3] = man.z + 1000; + + rc.values[4] = decode_switch_pos_n(man.buttons, 0) * 1000 + 1000; + rc.values[5] = decode_switch_pos_n(man.buttons, 1) * 1000 + 1000; + rc.values[6] = decode_switch_pos_n(man.buttons, 2) * 1000 + 1000; + rc.values[7] = decode_switch_pos_n(man.buttons, 3) * 1000 + 1000; + rc.values[8] = decode_switch_pos_n(man.buttons, 4) * 1000 + 1000; + rc.values[9] = decode_switch_pos_n(man.buttons, 5) * 1000 + 1000; if (_rc_pub <= 0) { _rc_pub = orb_advertise(ORB_ID(input_rc), &rc); From 81240d604096d80871a925199692f78a68bb4778 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 May 2015 23:31:58 +0200 Subject: [PATCH 10/84] commander: Initial RC detection is only an info string --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5f022370a0..45a26620a1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1718,7 +1718,7 @@ int commander_thread_main(int argc, char *argv[]) /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "detected RC signal first time"); + mavlink_log_info(mavlink_fd, "Detected RC signal first time"); status_changed = true; } else { From 27470930286e2da753958dbfbfc3ef1afa7340e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 16:34:21 +0200 Subject: [PATCH 11/84] vehicle status: Rename field which controls RC input mode --- msg/vehicle_status.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 3fe2426593..21a1d7a424 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -126,7 +126,7 @@ bool rc_signal_lost # true if RC reception lost uint64 rc_signal_lost_timestamp # Time at which the RC reception was lost bool rc_signal_lost_cmd # true if RC lost mode is commanded bool rc_input_blocked # set if RC input should be ignored temporarily -uint8 rc_input_off # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping. +uint8 rc_input_mode # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping. bool data_link_lost # datalink to GCS lost bool data_link_lost_cmd # datalink to GCS lost mode commanded From 6253f6154f6cc13edd7d3a09b9b2fb5b896ad00e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 16:34:42 +0200 Subject: [PATCH 12/84] commander: rename field for RC input mode --- src/modules/commander/commander.cpp | 12 ++++++------ src/modules/commander/commander_params.c | 2 +- src/modules/commander/state_machine_helper.cpp | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 45a26620a1..b81c9c1d30 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -889,7 +889,7 @@ int commander_thread_main(int argc, char *argv[]) status.condition_landed = true; // initialize to safe value // We want to accept RC inputs as default status.rc_input_blocked = false; - status.rc_input_off = vehicle_status_s::RC_IN_MODE_DEFAULT; + status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT; status.main_state =vehicle_status_s::MAIN_STATE_MANUAL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status.arming_state = vehicle_status_s::ARMING_STATE_INIT; @@ -1134,7 +1134,7 @@ int commander_thread_main(int argc, char *argv[]) } // Run preflight check - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_off, !status.circuit_breaker_engaged_gpsfailure_check); + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check); if (!status.condition_system_sensors_initialized) { set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune } @@ -1237,7 +1237,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_rc_loss_timeout, &rc_loss_timeout); param_get(_param_rc_in_off, &rc_in_off); - status.rc_input_off = rc_in_off; + status.rc_input_mode = rc_in_off; param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); param_get(_param_ef_throttle_thres, &ef_throttle_thres); param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); @@ -1311,7 +1311,7 @@ int commander_thread_main(int argc, char *argv[]) /* provide RC and sensor status feedback to the user */ (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, - !(status.rc_input_off == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; @@ -1713,7 +1713,7 @@ int commander_thread_main(int argc, char *argv[]) } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination /* RC input check */ - if (!(status.rc_input_off == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 && + if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { @@ -2777,7 +2777,7 @@ void *commander_low_prio_loop(void *arg) } status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, - !(status.rc_input_off == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 2225b00d35..bc8f833aef 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -197,7 +197,7 @@ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5); PARAM_DEFINE_INT32(COM_AUTOS_PAR, 1); /** - * Disable RC control input + * RC control input mode * * The default value of 0 requires a valid RC transmitter setup. * Setting this to 1 disables RC input handling and the associated checks. A value of diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 0dedd22282..a2086e9577 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -692,5 +692,5 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) checkAirspeed = true; } - return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_off == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true); + return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true); } From c6fe4fd35af11563738defe44ea4696c515e79ab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 16:34:57 +0200 Subject: [PATCH 13/84] mavlink: rename field for RC input mode --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9be21b25d7..a7d168efc0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1499,7 +1499,7 @@ Mavlink::task_main(int argc, char *argv[]) /* switch HIL mode if required */ set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON); - set_manual_input_mode_generation(status.rc_input_off == vehicle_status_s::RC_IN_MODE_GENERATED); + set_manual_input_mode_generation(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED); } /* check for requested subscriptions */ From 3883b7113213b6065103e7e32e8ccb519a0405e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 May 2015 13:15:48 +0200 Subject: [PATCH 14/84] pwm handler: Support channel reversal --- src/modules/systemlib/pwm_limit/pwm_limit.c | 21 ++++++++++++++++++--- src/modules/systemlib/pwm_limit/pwm_limit.h | 2 +- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index c733a53d7a..adcfb703c0 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -53,7 +53,9 @@ void pwm_limit_init(pwm_limit_t *limit) return; } -void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) +void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t reverse_mask, + const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, + const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) { /* first evaluate state changes */ @@ -134,7 +136,13 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ ramp_min_pwm = min_pwm[i]; } - effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; + float control_value = output[i]; + + if (reverse_mask & (1 << i)) { + control_value = -1.0f * control_value; + } + + effective_pwm[i] = control_value * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; /* last line of defense against invalid inputs */ if (effective_pwm[i] < ramp_min_pwm) { @@ -147,7 +155,14 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ break; case PWM_LIMIT_STATE_ON: for (unsigned i=0; i Date: Sat, 23 May 2015 13:16:05 +0200 Subject: [PATCH 15/84] IO firmware: Support pwm reversal --- src/modules/px4iofirmware/mixer.cpp | 5 +++-- src/modules/px4iofirmware/protocol.h | 2 ++ src/modules/px4iofirmware/px4io.h | 2 ++ src/modules/px4iofirmware/registers.c | 5 +++++ 4 files changed, 12 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index f14599a247..1bb25104fa 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -234,7 +234,7 @@ mixer_tick(void) in_mixer = false; /* the pwm limit call takes care of out of band errors */ - pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + pwm_limit_calc(should_arm, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; @@ -272,8 +272,9 @@ mixer_tick(void) if (mixer_servos_armed && should_arm) { /* update the servo outputs. */ - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { up_pwm_servo_set(i, r_page_servos[i]); + } /* set S.BUS1 or S.BUS2 outputs */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index cbafa36410..a649920320 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -233,6 +233,8 @@ enum { /* DSM bind states */ #define PX4IO_P_SETUP_FORCE_SAFETY_ON 14 /* force safety switch into 'disarmed' (PWM disabled state) */ #define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ +#define PX4IO_P_SETUP_PWM_REVERSE 15 /**< Bitmask to reverse PWM channels 1-8 */ + /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ #define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index a7ac74c33e..b38d374b9a 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -111,6 +111,8 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #endif #define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] +#define r_setup_pwm_reverse r_page_setup[PX4IO_P_SETUP_PWM_REVERSE] + #define r_control_values (&r_page_controls[0]) /* diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a8009da414..977bc59fff 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -173,6 +173,7 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_REBOOT_BL] = 0, [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, [PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0, + [PX4IO_P_SETUP_PWM_REVERSE] = 0, }; #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 @@ -622,6 +623,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) } break; + case PX4IO_P_SETUP_PWM_REVERSE: + r_page_setup[PX4IO_P_SETUP_PWM_REVERSE] = value; + break; + default: return -1; } From e4a5ceb4293be165d38629c27ba83a4e47cf8159 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 May 2015 13:16:34 +0200 Subject: [PATCH 16/84] FMU driver: Support pwm reversal, prepare config parameters --- src/drivers/px4fmu/fmu.cpp | 4 +- src/drivers/px4fmu/module.mk | 3 +- src/drivers/px4fmu/px4fmu_params.c | 109 +++++++++++++++++++++++++++++ 3 files changed, 114 insertions(+), 2 deletions(-) create mode 100644 src/drivers/px4fmu/px4fmu_params.c diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 92afc7cd74..154bc5f78f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -157,6 +157,7 @@ private: uint16_t _disarmed_pwm[_max_actuators]; uint16_t _min_pwm[_max_actuators]; uint16_t _max_pwm[_max_actuators]; + uint16_t _reverse_pwm_mask; unsigned _num_failsafe_set; unsigned _num_disarmed_set; @@ -269,6 +270,7 @@ PX4FMU::PX4FMU() : _pwm_limit{}, _failsafe_pwm{0}, _disarmed_pwm{0}, + _reverse_pwm_mask(0), _num_failsafe_set(0), _num_disarmed_set(0) { @@ -684,7 +686,7 @@ PX4FMU::task_main() uint16_t pwm_limited[num_outputs]; /* the PWM limit call takes care of out of band errors and constrains */ - pwm_limit_calc(_servo_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); + pwm_limit_calc(_servo_armed, num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); /* output to the servos */ for (unsigned i = 0; i < num_outputs; i++) { diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index a06323a52f..5a2806f4b5 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -3,7 +3,8 @@ # MODULE_COMMAND = fmu -SRCS = fmu.cpp +SRCS = fmu.cpp \ + px4fmu_params.c MODULE_STACKSIZE = 1200 diff --git a/src/drivers/px4fmu/px4fmu_params.c b/src/drivers/px4fmu/px4fmu_params.c new file mode 100644 index 0000000000..4df1d92199 --- /dev/null +++ b/src/drivers/px4fmu/px4fmu_params.c @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4fmu_params.c + * + * Parameters defined by the PX4FMU driver + * + * @author Lorenz Meier + */ + +#include +#include + +/** + * Inverter for main output channel 1 + * + * Set to 1 to invert the channel. + * + * @min 0 + * @max 1 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_SCALE1, 1); + +/** + * Inverter for main output channel 2 + * + * Set to 1 to invert the channel. + * + * @min 0 + * @max 1 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_SCALE2, 1); + +/** + * Inverter for main output channel 3 + * + * Set to 1 to invert the channel. + * + * @min 0 + * @max 1 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_SCALE3, 1); + +/** + * Inverter for main output channel 4 + * + * Set to 1 to invert the channel. + * + * @min 0 + * @max 1 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_SCALE4, 1); + +/** + * Inverter for main output channel 5 + * + * Set to 1 to invert the channel. + * + * @min 0 + * @max 1 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_SCALE5, 1); + +/** + * Inverter for main output channel 6 + * + * Set to 1 to invert the channel. + * + * @min 0 + * @max 1 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_SCALE6, 1); From a1232083fc87a9d4f598aae32692b339b0e4e8bd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 May 2015 13:16:57 +0200 Subject: [PATCH 17/84] IO driver: Support pwm reversal, prepare config parameters --- src/drivers/px4io/module.mk | 3 +- src/drivers/px4io/px4io_params.c | 131 +++++++++++++++++++++++++++++++ 2 files changed, 133 insertions(+), 1 deletion(-) create mode 100644 src/drivers/px4io/px4io_params.c diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk index 55f8037098..9033d24b61 100644 --- a/src/drivers/px4io/module.mk +++ b/src/drivers/px4io/module.mk @@ -40,7 +40,8 @@ MODULE_COMMAND = px4io SRCS = px4io.cpp \ px4io_uploader.cpp \ px4io_serial.cpp \ - px4io_i2c.cpp + px4io_i2c.cpp \ + px4io_params.c # XXX prune to just get UART registers INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common diff --git a/src/drivers/px4io/px4io_params.c b/src/drivers/px4io/px4io_params.c new file mode 100644 index 0000000000..ec3db8f8cb --- /dev/null +++ b/src/drivers/px4io/px4io_params.c @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4io_params.c + * + * Parameters defined by the PX4IO driver + * + * @author Lorenz Meier + */ + +#include +#include + +/** + * Pre-scaler / Inverter for main output channel 1 + * + * Set to 1 to invert the channel. + * + * @min 0 + * @max 1 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_SCALE1, 1); + +/** + * Pre-scaler / Inverter for main output channel 2 + * + * Set to 1 to invert the channel. + * + * @min 0 + * @max 1 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_SCALE2, 1); + +/** + * Pre-scaler / Inverter for main output channel 3 + * + * Set to 1 to invert the channel. + * + * @min 0 + * @max 1 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_SCALE3, 1); + +/** + * Pre-scaler / Inverter for main output channel 4 + * + * Set to 1 to invert the channel. + * + * @min 0 + * @max 1 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_SCALE4, 1); + +/** + * Pre-scaler / Inverter for main output channel 5 + * + * Set to 1 to invert the channel. + * + * @min 0 + * @max 1 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_SCALE5, 1); + +/** + * Pre-scaler / Inverter for main output channel 6 + * + * Set to 1 to invert the channel. + * + * @min 0 + * @max 1 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_SCALE6, 1); + +/** + * Pre-scaler / Inverter for main output channel 7 + * + * Set to 1 to invert the channel. + * + * @min 0 + * @max 1 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_SCALE7, 1); + +/** + * Pre-scaler / Inverter for main output channel 8 + * + * Set to 1 to invert the channel. + * + * @min 0 + * @max 1 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_SCALE8, 1); From 30cd3e1d791e8dd4c8d0dabb1248a2a01e510a0b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 May 2015 13:17:22 +0200 Subject: [PATCH 18/84] Mixer test: Support channel reversal --- src/systemcmds/tests/test_mixer.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 19aa059707..e9500d2d16 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -78,6 +78,7 @@ int test_mixer(int argc, char *argv[]) uint16_t r_page_servo_control_max[output_max]; uint16_t r_page_servos[output_max]; uint16_t servo_predicted[output_max]; + int16_t reverse_pwm_mask = 0; warnx("testing mixer"); @@ -204,8 +205,8 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, - r_page_servos, &pwm_limit); + pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { @@ -250,7 +251,7 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); warnx("mixed %d outputs (max %d)", mixed, output_max); @@ -277,7 +278,7 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); @@ -313,7 +314,7 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); From 3b3e98ed19c646aa730f85a277b88e3f7437ffee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 May 2015 14:56:04 +0200 Subject: [PATCH 19/84] Rename FMU params --- src/drivers/px4fmu/px4fmu_params.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/drivers/px4fmu/px4fmu_params.c b/src/drivers/px4fmu/px4fmu_params.c index 4df1d92199..461d7cb2f8 100644 --- a/src/drivers/px4fmu/px4fmu_params.c +++ b/src/drivers/px4fmu/px4fmu_params.c @@ -51,7 +51,7 @@ * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_SCALE1, 1); +PARAM_DEFINE_INT32(PWM_AUX_REV1, 1); /** * Inverter for main output channel 2 @@ -62,7 +62,7 @@ PARAM_DEFINE_INT32(PWM_AUX_SCALE1, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_SCALE2, 1); +PARAM_DEFINE_INT32(PWM_AUX_REV2, 1); /** * Inverter for main output channel 3 @@ -73,7 +73,7 @@ PARAM_DEFINE_INT32(PWM_AUX_SCALE2, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_SCALE3, 1); +PARAM_DEFINE_INT32(PWM_AUX_REV3, 1); /** * Inverter for main output channel 4 @@ -84,7 +84,7 @@ PARAM_DEFINE_INT32(PWM_AUX_SCALE3, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_SCALE4, 1); +PARAM_DEFINE_INT32(PWM_AUX_REV4, 1); /** * Inverter for main output channel 5 @@ -95,7 +95,7 @@ PARAM_DEFINE_INT32(PWM_AUX_SCALE4, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_SCALE5, 1); +PARAM_DEFINE_INT32(PWM_AUX_REV5, 1); /** * Inverter for main output channel 6 @@ -106,4 +106,4 @@ PARAM_DEFINE_INT32(PWM_AUX_SCALE5, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_SCALE6, 1); +PARAM_DEFINE_INT32(PWM_AUX_REV6, 1); From c46c150a3d6e9801257f2b00e92ac847832dce9e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 May 2015 14:56:16 +0200 Subject: [PATCH 20/84] Rename IO params --- src/drivers/px4io/px4io_params.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/drivers/px4io/px4io_params.c b/src/drivers/px4io/px4io_params.c index ec3db8f8cb..2dc92632a8 100644 --- a/src/drivers/px4io/px4io_params.c +++ b/src/drivers/px4io/px4io_params.c @@ -51,7 +51,7 @@ * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_MAIN_SCALE1, 1); +PARAM_DEFINE_INT32(PWM_MAIN_REV1, 1); /** * Pre-scaler / Inverter for main output channel 2 @@ -62,7 +62,7 @@ PARAM_DEFINE_INT32(PWM_MAIN_SCALE1, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_MAIN_SCALE2, 1); +PARAM_DEFINE_INT32(PWM_MAIN_REV2, 1); /** * Pre-scaler / Inverter for main output channel 3 @@ -73,7 +73,7 @@ PARAM_DEFINE_INT32(PWM_MAIN_SCALE2, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_MAIN_SCALE3, 1); +PARAM_DEFINE_INT32(PWM_MAIN_REV3, 1); /** * Pre-scaler / Inverter for main output channel 4 @@ -84,7 +84,7 @@ PARAM_DEFINE_INT32(PWM_MAIN_SCALE3, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_MAIN_SCALE4, 1); +PARAM_DEFINE_INT32(PWM_MAIN_REV4, 1); /** * Pre-scaler / Inverter for main output channel 5 @@ -95,7 +95,7 @@ PARAM_DEFINE_INT32(PWM_MAIN_SCALE4, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_MAIN_SCALE5, 1); +PARAM_DEFINE_INT32(PWM_MAIN_REV5, 1); /** * Pre-scaler / Inverter for main output channel 6 @@ -106,7 +106,7 @@ PARAM_DEFINE_INT32(PWM_MAIN_SCALE5, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_MAIN_SCALE6, 1); +PARAM_DEFINE_INT32(PWM_MAIN_REV6, 1); /** * Pre-scaler / Inverter for main output channel 7 @@ -117,7 +117,7 @@ PARAM_DEFINE_INT32(PWM_MAIN_SCALE6, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_MAIN_SCALE7, 1); +PARAM_DEFINE_INT32(PWM_MAIN_REV7, 1); /** * Pre-scaler / Inverter for main output channel 8 @@ -128,4 +128,4 @@ PARAM_DEFINE_INT32(PWM_MAIN_SCALE7, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_MAIN_SCALE8, 1); +PARAM_DEFINE_INT32(PWM_MAIN_REV8, 1); From b155e97a5475bb6b2d957d5fcff5da5a343a69ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 May 2015 14:56:25 +0200 Subject: [PATCH 21/84] Load IO params for mask --- src/drivers/px4io/px4io.cpp | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 176f5b241d..1cdd183a97 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1089,6 +1089,26 @@ PX4IO::task_main() param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max); param_get(param_find("RC_RSSI_PWM_MIN"), &_rssi_pwm_min); + /* + * Set invert mask for PWM outputs (does not apply to S.Bus) + */ + int16_t pwm_invert_mask = 0; + + for (unsigned i = 0; i < _max_actuators; i++) { + char pname[16]; + int32_t ival; + + /* fill the channel reverse mask from parameters */ + sprintf(pname, "PWM_MAIN_REV%d", i + 1); + param_t param_h = param_find(pname); + + if (param_h != PARAM_INVALID) { + param_get(param_h, &ival); + pwm_invert_mask |= ((int16_t)(ival != 0)) << i; + } + } + + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE, pwm_invert_mask); } } @@ -2095,7 +2115,15 @@ PX4IO::print_status(bool extended_status) for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i)); + uint16_t pwm_invert_mask = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE); + printf("\n"); + printf("reversed outputs: ["); + for (unsigned i = 0; i < _max_actuators; i++) { + printf("%s", (pwm_invert_mask & (1 << i)) ? "x" : "_"); + } + printf("]\n"); + uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); From 129aa35fcde593a37e7d33637a7979564cc83aea Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 May 2015 15:06:27 +0200 Subject: [PATCH 22/84] FMU driver: Load channel reverse mask from parameters --- src/drivers/px4fmu/fmu.cpp | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 154bc5f78f..54c955bb05 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -67,6 +67,7 @@ #include #include #include +#include #include #include @@ -77,6 +78,7 @@ #include #include #include +#include #ifdef HRT_PPM_CHANNEL @@ -132,6 +134,7 @@ private: unsigned _current_update_rate; int _task; int _armed_sub; + int _param_sub; orb_advert_t _outputs_pub; actuator_armed_s _armed; unsigned _num_outputs; @@ -254,6 +257,7 @@ PX4FMU::PX4FMU() : _current_update_rate(0), _task(-1), _armed_sub(-1), + _param_sub(-1), _outputs_pub(-1), _armed{}, _num_outputs(0), @@ -552,6 +556,7 @@ PX4FMU::task_main() _current_update_rate = 0; _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _param_sub = orb_subscribe(ORB_ID(parameter_update)); /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -725,6 +730,28 @@ PX4FMU::task_main() } } + orb_check(_param_sub, &updated); + + if (updated) { + parameter_update_s pupdate; + orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); + _reverse_pwm_mask = 0; + + for (unsigned i = 0; i < _max_actuators; i++) { + char pname[16]; + int32_t ival; + + /* fill the channel reverse mask from parameters */ + sprintf(pname, "PWM_AUX_REV%d", i + 1); + param_t param_h = param_find(pname); + + if (param_h != PARAM_INVALID) { + param_get(param_h, &ival); + _reverse_pwm_mask |= ((int16_t)(ival != 0)) << i; + } + } + } + #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data @@ -770,6 +797,7 @@ PX4FMU::task_main() } } ::close(_armed_sub); + ::close(_param_sub); /* make sure servos are off */ up_pwm_servo_deinit(); From 2dd94b7f0f7a9d5a4e2c929ae2a6c2ec65be7604 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 May 2015 15:18:13 +0200 Subject: [PATCH 23/84] FMU driver: auto param update --- src/drivers/px4fmu/fmu.cpp | 43 +++++++++++++++++------------- src/drivers/px4fmu/px4fmu_params.c | 12 ++++----- 2 files changed, 31 insertions(+), 24 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 54c955bb05..c757dcc29b 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -171,9 +171,10 @@ private: uint8_t control_group, uint8_t control_index, float &input); - void subscribe(); + void subscribe(); int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); int pwm_ioctl(file *filp, int cmd, unsigned long arg); + void update_pwm_rev_mask(); struct GPIOConfig { uint32_t input; @@ -549,6 +550,26 @@ PX4FMU::subscribe() } } +void +PX4FMU::update_pwm_rev_mask() +{ + _reverse_pwm_mask = 0; + + for (unsigned i = 0; i < _max_actuators; i++) { + char pname[16]; + int32_t ival; + + /* fill the channel reverse mask from parameters */ + sprintf(pname, "PWM_AUX_REV%d", i + 1); + param_t param_h = param_find(pname); + + if (param_h != PARAM_INVALID) { + param_get(param_h, &ival); + _reverse_pwm_mask |= ((int16_t)(ival != 0)) << i; + } + } +} + void PX4FMU::task_main() { @@ -574,7 +595,7 @@ PX4FMU::task_main() /* initialize PWM limit lib */ pwm_limit_init(&_pwm_limit); - log("starting"); + update_pwm_rev_mask(); /* loop until killed */ while (!_task_should_exit) { @@ -731,25 +752,11 @@ PX4FMU::task_main() } orb_check(_param_sub, &updated); - if (updated) { parameter_update_s pupdate; orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); - _reverse_pwm_mask = 0; - - for (unsigned i = 0; i < _max_actuators; i++) { - char pname[16]; - int32_t ival; - - /* fill the channel reverse mask from parameters */ - sprintf(pname, "PWM_AUX_REV%d", i + 1); - param_t param_h = param_find(pname); - - if (param_h != PARAM_INVALID) { - param_get(param_h, &ival); - _reverse_pwm_mask |= ((int16_t)(ival != 0)) << i; - } - } + + update_pwm_rev_mask(); } #ifdef HRT_PPM_CHANNEL diff --git a/src/drivers/px4fmu/px4fmu_params.c b/src/drivers/px4fmu/px4fmu_params.c index 461d7cb2f8..4377c22e77 100644 --- a/src/drivers/px4fmu/px4fmu_params.c +++ b/src/drivers/px4fmu/px4fmu_params.c @@ -51,7 +51,7 @@ * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_REV1, 1); +PARAM_DEFINE_INT32(PWM_AUX_REV1, 0); /** * Inverter for main output channel 2 @@ -62,7 +62,7 @@ PARAM_DEFINE_INT32(PWM_AUX_REV1, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_REV2, 1); +PARAM_DEFINE_INT32(PWM_AUX_REV2, 0); /** * Inverter for main output channel 3 @@ -73,7 +73,7 @@ PARAM_DEFINE_INT32(PWM_AUX_REV2, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_REV3, 1); +PARAM_DEFINE_INT32(PWM_AUX_REV3, 0); /** * Inverter for main output channel 4 @@ -84,7 +84,7 @@ PARAM_DEFINE_INT32(PWM_AUX_REV3, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_REV4, 1); +PARAM_DEFINE_INT32(PWM_AUX_REV4, 0); /** * Inverter for main output channel 5 @@ -95,7 +95,7 @@ PARAM_DEFINE_INT32(PWM_AUX_REV4, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_REV5, 1); +PARAM_DEFINE_INT32(PWM_AUX_REV5, 0); /** * Inverter for main output channel 6 @@ -106,4 +106,4 @@ PARAM_DEFINE_INT32(PWM_AUX_REV5, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_REV6, 1); +PARAM_DEFINE_INT32(PWM_AUX_REV6, 0); From d54b9315544fc8db09a8e113e27029f5ef1363ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 May 2015 15:18:25 +0200 Subject: [PATCH 24/84] IO driver: auto update param --- src/drivers/px4io/px4io.cpp | 7 ++++++- src/drivers/px4io/px4io_params.c | 16 ++++++++-------- 2 files changed, 14 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1cdd183a97..bb2f85d61e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -281,6 +281,7 @@ private: int _t_actuator_armed; ///< system armed control topic int _t_vehicle_control_mode;///< vehicle control mode topic int _t_param; ///< parameter update topic + bool _param_update_force; ///< force a parameter update int _t_vehicle_command; ///< vehicle command topic /* advertised topics */ @@ -514,6 +515,7 @@ PX4IO::PX4IO(device::Device *interface) : _t_actuator_armed(-1), _t_vehicle_control_mode(-1), _t_param(-1), + _param_update_force(false), _t_vehicle_command(-1), _to_input_rc(0), _to_outputs(0), @@ -917,6 +919,8 @@ PX4IO::task_main() fds[0].fd = _t_actuator_controls_0; fds[0].events = POLLIN; + _param_update_force = true; + /* lock against the ioctl handler */ lock(); @@ -1017,7 +1021,8 @@ PX4IO::task_main() */ orb_check(_t_param, &updated); - if (updated) { + if (updated || _param_update_force) { + _param_update_force = false; parameter_update_s pupdate; orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); diff --git a/src/drivers/px4io/px4io_params.c b/src/drivers/px4io/px4io_params.c index 2dc92632a8..67e9d3cb3b 100644 --- a/src/drivers/px4io/px4io_params.c +++ b/src/drivers/px4io/px4io_params.c @@ -51,7 +51,7 @@ * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_MAIN_REV1, 1); +PARAM_DEFINE_INT32(PWM_MAIN_REV1, 0); /** * Pre-scaler / Inverter for main output channel 2 @@ -62,7 +62,7 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV1, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_MAIN_REV2, 1); +PARAM_DEFINE_INT32(PWM_MAIN_REV2, 0); /** * Pre-scaler / Inverter for main output channel 3 @@ -73,7 +73,7 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV2, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_MAIN_REV3, 1); +PARAM_DEFINE_INT32(PWM_MAIN_REV3, 0); /** * Pre-scaler / Inverter for main output channel 4 @@ -84,7 +84,7 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV3, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_MAIN_REV4, 1); +PARAM_DEFINE_INT32(PWM_MAIN_REV4, 0); /** * Pre-scaler / Inverter for main output channel 5 @@ -95,7 +95,7 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV4, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_MAIN_REV5, 1); +PARAM_DEFINE_INT32(PWM_MAIN_REV5, 0); /** * Pre-scaler / Inverter for main output channel 6 @@ -106,7 +106,7 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV5, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_MAIN_REV6, 1); +PARAM_DEFINE_INT32(PWM_MAIN_REV6, 0); /** * Pre-scaler / Inverter for main output channel 7 @@ -117,7 +117,7 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV6, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_MAIN_REV7, 1); +PARAM_DEFINE_INT32(PWM_MAIN_REV7, 0); /** * Pre-scaler / Inverter for main output channel 8 @@ -128,4 +128,4 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV7, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_MAIN_REV8, 1); +PARAM_DEFINE_INT32(PWM_MAIN_REV8, 0); From e3d7f0042a708fa97bb301653252b015f9ec56e5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 May 2015 11:41:42 +0200 Subject: [PATCH 25/84] FMU params: better documentation --- src/drivers/px4fmu/px4fmu_params.c | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/drivers/px4fmu/px4fmu_params.c b/src/drivers/px4fmu/px4fmu_params.c index 4377c22e77..ef345f193d 100644 --- a/src/drivers/px4fmu/px4fmu_params.c +++ b/src/drivers/px4fmu/px4fmu_params.c @@ -43,9 +43,9 @@ #include /** - * Inverter for main output channel 1 + * Invert direction of aux output channel 1 * - * Set to 1 to invert the channel. + * Set to 1 to invert the channel, 0 for default direction. * * @min 0 * @max 1 @@ -54,9 +54,9 @@ PARAM_DEFINE_INT32(PWM_AUX_REV1, 0); /** - * Inverter for main output channel 2 + * Invert direction of aux output channel 2 * - * Set to 1 to invert the channel. + * Set to 1 to invert the channel, 0 for default direction. * * @min 0 * @max 1 @@ -65,9 +65,9 @@ PARAM_DEFINE_INT32(PWM_AUX_REV1, 0); PARAM_DEFINE_INT32(PWM_AUX_REV2, 0); /** - * Inverter for main output channel 3 + * Invert direction of aux output channel 3 * - * Set to 1 to invert the channel. + * Set to 1 to invert the channel, 0 for default direction. * * @min 0 * @max 1 @@ -76,9 +76,9 @@ PARAM_DEFINE_INT32(PWM_AUX_REV2, 0); PARAM_DEFINE_INT32(PWM_AUX_REV3, 0); /** - * Inverter for main output channel 4 + * Invert direction of aux output channel 4 * - * Set to 1 to invert the channel. + * Set to 1 to invert the channel, 0 for default direction. * * @min 0 * @max 1 @@ -87,9 +87,9 @@ PARAM_DEFINE_INT32(PWM_AUX_REV3, 0); PARAM_DEFINE_INT32(PWM_AUX_REV4, 0); /** - * Inverter for main output channel 5 + * Invert direction of aux output channel 5 * - * Set to 1 to invert the channel. + * Set to 1 to invert the channel, 0 for default direction. * * @min 0 * @max 1 @@ -98,9 +98,9 @@ PARAM_DEFINE_INT32(PWM_AUX_REV4, 0); PARAM_DEFINE_INT32(PWM_AUX_REV5, 0); /** - * Inverter for main output channel 6 + * Invert direction of aux output channel 6 * - * Set to 1 to invert the channel. + * Set to 1 to invert the channel, 0 for default direction. * * @min 0 * @max 1 From 983243933d2a291367a79dd4cefe9c55a2ca958c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 May 2015 11:42:00 +0200 Subject: [PATCH 26/84] PX4IO params: better documentation --- src/drivers/px4io/px4io_params.c | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/drivers/px4io/px4io_params.c b/src/drivers/px4io/px4io_params.c index 67e9d3cb3b..057bcaf150 100644 --- a/src/drivers/px4io/px4io_params.c +++ b/src/drivers/px4io/px4io_params.c @@ -43,9 +43,9 @@ #include /** - * Pre-scaler / Inverter for main output channel 1 + * Invert direction of main output channel 1 * - * Set to 1 to invert the channel. + * Set to 1 to invert the channel, 0 for default direction. * * @min 0 * @max 1 @@ -54,9 +54,9 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV1, 0); /** - * Pre-scaler / Inverter for main output channel 2 + * Invert direction of main output channel 2 * - * Set to 1 to invert the channel. + * Set to 1 to invert the channel, 0 for default direction. * * @min 0 * @max 1 @@ -65,9 +65,9 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV1, 0); PARAM_DEFINE_INT32(PWM_MAIN_REV2, 0); /** - * Pre-scaler / Inverter for main output channel 3 + * Invert direction of main output channel 3 * - * Set to 1 to invert the channel. + * Set to 1 to invert the channel, 0 for default direction. * * @min 0 * @max 1 @@ -76,9 +76,9 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV2, 0); PARAM_DEFINE_INT32(PWM_MAIN_REV3, 0); /** - * Pre-scaler / Inverter for main output channel 4 + * Invert direction of main output channel 4 * - * Set to 1 to invert the channel. + * Set to 1 to invert the channel, 0 for default direction. * * @min 0 * @max 1 @@ -87,9 +87,9 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV3, 0); PARAM_DEFINE_INT32(PWM_MAIN_REV4, 0); /** - * Pre-scaler / Inverter for main output channel 5 + * Invert direction of main output channel 5 * - * Set to 1 to invert the channel. + * Set to 1 to invert the channel, 0 for default direction. * * @min 0 * @max 1 @@ -98,9 +98,9 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV4, 0); PARAM_DEFINE_INT32(PWM_MAIN_REV5, 0); /** - * Pre-scaler / Inverter for main output channel 6 + * Invert direction of main output channel 6 * - * Set to 1 to invert the channel. + * Set to 1 to invert the channel, 0 for default direction. * * @min 0 * @max 1 @@ -109,9 +109,9 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV5, 0); PARAM_DEFINE_INT32(PWM_MAIN_REV6, 0); /** - * Pre-scaler / Inverter for main output channel 7 + * Invert direction of main output channel 7 * - * Set to 1 to invert the channel. + * Set to 1 to invert the channel, 0 for default direction. * * @min 0 * @max 1 @@ -120,9 +120,9 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV6, 0); PARAM_DEFINE_INT32(PWM_MAIN_REV7, 0); /** - * Pre-scaler / Inverter for main output channel 8 + * Invert direction of main output channel 8 * - * Set to 1 to invert the channel. + * Set to 1 to invert the channel, 0 for default direction. * * @min 0 * @max 1 From 54fde63d6656318405d06a82e387cff88f44362e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 17:31:37 +0200 Subject: [PATCH 27/84] FMU driver: Code style fix --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index c757dcc29b..b340694bf0 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -755,7 +755,7 @@ PX4FMU::task_main() if (updated) { parameter_update_s pupdate; orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); - + update_pwm_rev_mask(); } From db7d0ef51f1f44717e4e72bd50153ee4fa29da7d Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Tue, 26 May 2015 15:28:25 -0600 Subject: [PATCH 28/84] add a new rotation to lib/conversion --- src/lib/conversion/rotation.cpp | 7 ++++++- src/lib/conversion/rotation.h | 6 ++++-- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 7418ba4ef1..ca82ddd5e8 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -41,7 +41,7 @@ #include "rotation.h" __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix) +get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix) { float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; @@ -199,5 +199,10 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) y = -y; return; } + case ROTATION_PITCH_90_YAW_180: { + tmp = x; x = z; z = tmp; + y = -y; + return; + } } } diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index ef0f41c3a8..8128551e51 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -76,6 +76,7 @@ enum Rotation { ROTATION_PITCH_270 = 25, ROTATION_ROLL_270_YAW_270 = 26, ROTATION_ROLL_180_PITCH_270 = 27, + ROTATION_PITCH_90_YAW_180 = 28, ROTATION_MAX }; @@ -113,14 +114,15 @@ const rot_lookup_t rot_lookup[] = { { 0, 90, 0 }, { 0, 270, 0 }, {270, 0, 270 }, - {180, 270, 0 } + {180, 270, 0 }, + { 0, 90, 180 } }; /** * Get the rotation matrix */ __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix); +get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); /** From 321e1c1f7bffecf7a7b6ae0e56672c80718d6808 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Mon, 25 May 2015 18:09:01 +0100 Subject: [PATCH 29/84] navigator: stop complaining about some geofence not cleared --- src/modules/navigator/navigator_main.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 042f926d3a..295172ebb2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -268,10 +268,9 @@ Navigator::task_main() } else { mavlink_log_info(_mavlink_fd, "No geofence set"); - if (_geofence.clearDm() > 0) - warnx("Geofence cleared"); - else + if (_geofence.clearDm() != OK) { warnx("Could not clear geofence"); + } } /* do subscriptions */ From 43d22619f6cbb3bfc349621749ade1909d6fc174 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Mon, 25 May 2015 18:28:27 +0100 Subject: [PATCH 30/84] mavlink: no need to shout about disabling hardware flow control --- src/modules/mavlink/mavlink_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index a7d168efc0..348185e693 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -746,7 +746,7 @@ Mavlink::get_free_tx_buf() if (_last_write_try_time != 0 && hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL && _last_write_success_time != _last_write_try_time) { - warnx("DISABLING HARDWARE FLOW CONTROL"); + warnx("Disabling hardware flow control"); enable_flow_control(false); } } @@ -877,7 +877,7 @@ Mavlink::handle_message(const mavlink_message_t *msg) /* handle packet with parameter component */ _parameters_manager->handle_message(msg); - + /* handle packet with ftp component */ _mavlink_ftp->handle_message(msg); @@ -1421,7 +1421,7 @@ Mavlink::task_main(int argc, char *argv[]) _mavlink_ftp = (MavlinkFTP *) MavlinkFTP::new_instance(this); _mavlink_ftp->set_interval(interval_from_rate(80.0f)); LL_APPEND(_streams, _mavlink_ftp); - + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on * remote requests rate. Rate specified here controls how much bandwidth we will reserve for * mission messages. */ From 1734b97635687a43e17bcca6a754c800de79597b Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Mon, 25 May 2015 18:34:31 +0100 Subject: [PATCH 31/84] px4flow: tell the user which PX4FLOW couldn't be connected --- src/drivers/px4flow/px4flow.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index b343a3e30a..ac20f18327 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -674,7 +674,7 @@ fail: g_dev = nullptr; } - errx(1, "no PX4 FLOW connected"); + errx(1, "no PX4FLOW connected over I2C"); } /** From 11f80ceb64bc6e508a259299b20b342e54b2dc1e Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Mon, 25 May 2015 19:01:34 +0100 Subject: [PATCH 32/84] land_detector: get rid of one dot at a time --- .../land_detector/land_detector_main.cpp | 28 +++++++++++-------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index c3829bb70d..e0f96ec77f 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -151,20 +151,26 @@ static int land_detector_start(const char *mode) /* avoid memory fragmentation by not exiting start handler until the task has fully started */ const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout - while (!land_detector_task->isRunning()) { - usleep(50000); - printf("."); - fflush(stdout); - - if (hrt_absolute_time() > timeout) { - err(1, "start failed - timeout"); - land_detector_stop(); - exit(1); + /* avoid printing dots just yet and do one sleep before the first check */ + usleep(10000); + + /* check if the waiting involving dots and a newline are still needed */ + if (!land_detector_task->isRunning()) { + while (!land_detector_task->isRunning()) { + + printf("."); + fflush(stdout); + usleep(50000); + + if (hrt_absolute_time() > timeout) { + err(1, "start failed - timeout"); + land_detector_stop(); + exit(1); + } } + printf("\n"); } - printf("\n"); - //Remember current active mode strncpy(_currentMode, mode, 12); From a7902c02a94e4e062f9f4a4e36dbf85e8b105ae7 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Mon, 25 May 2015 19:08:51 +0100 Subject: [PATCH 33/84] position_estimator_inav: be a bit more informative about this ominous offs --- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 437395b1f1..f6d096cec0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -389,8 +389,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; - warnx("baro offs: %d", (int)baro_offset); - mavlink_log_info(mavlink_fd, "[inav] baro offs: %d", (int)baro_offset); + warnx("baro offset: %d m", (int)baro_offset); + mavlink_log_info(mavlink_fd, "[inav] baro offset: %d m", (int)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } From e2277bbe2d054da40c2b28583ff476ba42675bd1 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Mon, 25 May 2015 19:09:28 +0100 Subject: [PATCH 34/84] fw_att_control: don't brag about being running --- src/modules/fw_att_control/fw_att_control_main.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 0a8d07fed9..165a4b48f2 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -619,11 +619,6 @@ FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) void FixedwingAttitudeControl::task_main() { - - /* inform about start */ - warnx("Initializing.."); - fflush(stdout); - /* * do subscriptions */ From d3fe6fd2562323e2e66e18c946f0710b44828508 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Mon, 25 May 2015 19:09:45 +0100 Subject: [PATCH 35/84] fw_att_control: whitespace --- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 165a4b48f2..3a2b478a26 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -520,7 +520,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll() orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); if (vcontrol_mode_updated) { - + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); } } From 96bfe85ceb19bb800a01ad5a488c8f1ccd183ee6 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Mon, 25 May 2015 19:10:05 +0100 Subject: [PATCH 36/84] fw_att_control: another ugly way to prevent a newline --- .../fw_att_control/fw_att_control_main.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 3a2b478a26..8b0d452a3e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1149,14 +1149,17 @@ int fw_att_control_main(int argc, char *argv[]) err(1, "start failed"); } - /* avoid memory fragmentation by not exiting start handler until the task has fully started */ - while (att_control::g_control == nullptr || !att_control::g_control->task_running()) { - usleep(50000); - printf("."); - fflush(stdout); + /* check if the waiting is necessary at all */ + if (att_control::g_control == nullptr || !att_control::g_control->task_running()) { + + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + while (att_control::g_control == nullptr || !att_control::g_control->task_running()) { + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); } - printf("\n"); - exit(0); } From 465ea8abe36c3bd4dd7fd5966cfc98a29ec64d71 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 18:11:03 +0200 Subject: [PATCH 37/84] Move actuator_outputs topic into message generation --- msg/actuator_outputs.msg | 5 ++ src/modules/uORB/topics/actuator_outputs.h | 73 ---------------------- 2 files changed, 5 insertions(+), 73 deletions(-) create mode 100644 msg/actuator_outputs.msg delete mode 100644 src/modules/uORB/topics/actuator_outputs.h diff --git a/msg/actuator_outputs.msg b/msg/actuator_outputs.msg new file mode 100644 index 0000000000..00a3c35b79 --- /dev/null +++ b/msg/actuator_outputs.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_OUTPUTS = 16 +uint8 NUM_ACTUATOR_OUTPUT_GROUPS = 4 # for sanity checking +uint64 timestamp # output timestamp in us since system boot +uint32 noutputs # valid outputs +float32[16] output # output data, in natural output units diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h deleted file mode 100644 index c6fbaaed5a..0000000000 --- a/src/modules/uORB/topics/actuator_outputs.h +++ /dev/null @@ -1,73 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file actuator_outputs.h - * - * Actuator output values. - * - * Values published to these topics are the outputs of the control mixing - * system as sent to the actuators (servos, motors, etc.) that operate - * the vehicle. - * - * Each topic can be published by a single output driver. - */ - -#ifndef TOPIC_ACTUATOR_OUTPUTS_H -#define TOPIC_ACTUATOR_OUTPUTS_H - -#include -#include "../uORB.h" - -#define NUM_ACTUATOR_OUTPUTS 16 -#define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */ - -/** - * @addtogroup topics - * @{ - */ - -struct actuator_outputs_s { - uint64_t timestamp; /**< output timestamp in us since system boot */ - float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */ - unsigned noutputs; /**< valid outputs */ -}; - -/** - * @} - */ - -/* actuator output sets; this list can be expanded as more drivers emerge */ -ORB_DECLARE(actuator_outputs); - -#endif From b980e34c3c473c8b6b19b67c18a23c9b4cc8411d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 18:11:56 +0200 Subject: [PATCH 38/84] Update uavcan app for generated actuator_outputs topic --- src/modules/uavcan/uavcan_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 474739bd66..ca92d09ecb 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -398,8 +398,8 @@ int UavcanNode::run() (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) && orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK && !_test_in_progress) { - if (_actuator_direct.nvalues > NUM_ACTUATOR_OUTPUTS) { - _actuator_direct.nvalues = NUM_ACTUATOR_OUTPUTS; + if (_actuator_direct.nvalues > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) { + _actuator_direct.nvalues = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; } memcpy(&_outputs.output[0], &_actuator_direct.values[0], _actuator_direct.nvalues*sizeof(float)); @@ -410,7 +410,7 @@ int UavcanNode::run() // can we mix? if (_test_in_progress) { memset(&_outputs, 0, sizeof(_outputs)); - if (_test_motor.motor_number < NUM_ACTUATOR_OUTPUTS) { + if (_test_motor.motor_number < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) { _outputs.output[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; _outputs.noutputs = _test_motor.motor_number+1; } From f5670c8ad6a5a5d470334cb5604cf967c27b87cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 18:12:40 +0200 Subject: [PATCH 39/84] Update actuator_direct topic to use message generation --- msg/actuator_direct.msg | 4 ++ src/modules/uORB/topics/actuator_direct.h | 69 ----------------------- 2 files changed, 4 insertions(+), 69 deletions(-) create mode 100644 msg/actuator_direct.msg delete mode 100644 src/modules/uORB/topics/actuator_direct.h diff --git a/msg/actuator_direct.msg b/msg/actuator_direct.msg new file mode 100644 index 0000000000..c798f5101f --- /dev/null +++ b/msg/actuator_direct.msg @@ -0,0 +1,4 @@ +uint8 NUM_ACTUATORS_DIRECT = 16 +uint64 timestamp # timestamp in us since system boot +uint32 nvalues # number of valid values +float32[16] values # actuator values, from -1 to 1 diff --git a/src/modules/uORB/topics/actuator_direct.h b/src/modules/uORB/topics/actuator_direct.h deleted file mode 100644 index 5f9d0f56dd..0000000000 --- a/src/modules/uORB/topics/actuator_direct.h +++ /dev/null @@ -1,69 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file actuator_direct.h - * - * Actuator direct values. - * - * Values published to this topic are the direct actuator values which - * should be passed to actuators, bypassing mixing - */ - -#ifndef TOPIC_ACTUATOR_DIRECT_H -#define TOPIC_ACTUATOR_DIRECT_H - -#include -#include "../uORB.h" - -#define NUM_ACTUATORS_DIRECT 16 - -/** - * @addtogroup topics - * @{ - */ - -struct actuator_direct_s { - uint64_t timestamp; /**< timestamp in us since system boot */ - float values[NUM_ACTUATORS_DIRECT]; /**< actuator values, from -1 to 1 */ - unsigned nvalues; /**< number of valid values */ -}; - -/** - * @} - */ - -/* actuator direct ORB */ -ORB_DECLARE(actuator_direct); - -#endif // TOPIC_ACTUATOR_DIRECT_H From f7c9e918b1fb933dd431076b53444c1d004099d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 18:13:38 +0200 Subject: [PATCH 40/84] Update airspeed topic to use message generation --- msg/airspeed.msg | 4 +++ src/modules/uORB/topics/airspeed.h | 40 ++++++++++++++++-------------- 2 files changed, 26 insertions(+), 18 deletions(-) create mode 100644 msg/airspeed.msg diff --git a/msg/airspeed.msg b/msg/airspeed.msg new file mode 100644 index 0000000000..8d6af2138d --- /dev/null +++ b/msg/airspeed.msg @@ -0,0 +1,4 @@ +uint64 timestamp # microseconds since system boot, needed to integrate +float32 indicated_airspeed_m_s # indicated airspeed in meters per second, -1 if unknown +float32 true_airspeed_m_s # true airspeed in meters per second, -1 if unknown +float32 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h index 676c37c778..df4b315146 100644 --- a/src/modules/uORB/topics/airspeed.h +++ b/src/modules/uORB/topics/airspeed.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-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,31 +31,37 @@ * ****************************************************************************/ -/** - * @file airspeed.h - * - * Definition of airspeed topic - */ +/* Auto-generated by genmsg_cpp from file /Users/lorenzmeier/src/Firmware/msg/airspeed.msg */ + -#ifndef TOPIC_AIRSPEED_H_ -#define TOPIC_AIRSPEED_H_ +#pragma once -#include #include +#include + + +#ifndef __cplusplus + +#endif /** * @addtogroup topics * @{ */ -/** - * Airspeed - */ + +#ifdef __cplusplus +struct __EXPORT airspeed_s { +#else struct airspeed_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ - float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */ - float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ - float air_temperature_celsius; /**< air temperature in degrees celsius, -1000 if unknown */ +#endif + uint64_t timestamp; + float indicated_airspeed_m_s; + float true_airspeed_m_s; + float air_temperature_celsius; +#ifdef __cplusplus + +#endif }; /** @@ -64,5 +70,3 @@ struct airspeed_s { /* register this as object request broker structure */ ORB_DECLARE(airspeed); - -#endif From e07731de7a5d3cf5f7a5deedd60556b761399bb7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 19:14:48 +0200 Subject: [PATCH 41/84] Move esc_status to generated topics --- msg/esc_report.msg | 11 +++ msg/esc_status.msg | 19 +++++ src/drivers/hott/messages.cpp | 4 +- src/drivers/mkblctrl/mkblctrl.cpp | 4 +- src/modules/uORB/topics/esc_status.h | 116 --------------------------- src/modules/uavcan/actuators/esc.cpp | 6 +- 6 files changed, 37 insertions(+), 123 deletions(-) create mode 100644 msg/esc_report.msg create mode 100644 msg/esc_status.msg delete mode 100644 src/modules/uORB/topics/esc_status.h diff --git a/msg/esc_report.msg b/msg/esc_report.msg new file mode 100644 index 0000000000..8525f3be15 --- /dev/null +++ b/msg/esc_report.msg @@ -0,0 +1,11 @@ +uint8 esc_vendor # Vendor of current ESC +uint32 esc_errorcount # Number of reported errors by ESC - if supported +int32 esc_rpm # Motor RPM, negative for reverse rotation [RPM] - if supported +float32 esc_voltage # Voltage measured from current ESC [V] - if supported +float32 esc_current # Current measured from current ESC [A] - if supported +float32 esc_temperature # Temperature measured from current ESC [degC] - if supported +float32 esc_setpoint # setpoint of current ESC +uint16 esc_setpoint_raw # setpoint of current ESC (Value sent to ESC) +uint16 esc_address # Address of current ESC (in most cases 1-8 / must be set by driver) +uint16 esc_version # Version of current ESC - if supported +uint16 esc_state # State of ESC - depend on Vendor diff --git a/msg/esc_status.msg b/msg/esc_status.msg new file mode 100644 index 0000000000..b54131756b --- /dev/null +++ b/msg/esc_status.msg @@ -0,0 +1,19 @@ +uint8 CONNECTED_ESC_MAX = 8 # The number of ESCs supported. Current (Q2/2013) we support 8 ESCs + +uint8 ESC_VENDOR_GENERIC = 0 # generic ESC +uint8 ESC_VENDOR_MIKROKOPTER = 1 # Mikrokopter +uint8 ESC_VENDOR_GRAUPNER_HOTT = 2 # Graupner HoTT ESC + +uint8 ESC_CONNECTION_TYPE_PPM = 0 # Traditional PPM ESC +uint8 ESC_CONNECTION_TYPE_SERIAL = 1 # Serial Bus connected ESC +uint8 ESC_CONNECTION_TYPE_ONESHOOT = 2 # One Shoot PPM +uint8 ESC_CONNECTION_TYPE_I2C = 3 # I2C +uint8 ESC_CONNECTION_TYPE_CAN = 4 # CAN-Bus + +uint16 counter # incremented by the writing thread everytime new data is stored +uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data + +uint8 esc_count # number of connected ESCs +uint8 esc_connectiontype # how ESCs connected to the system + +esc_report[8] esc diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index f1b12b0678..a5a923aeae 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -111,9 +111,9 @@ publish_gam_message(const uint8_t *buffer) // Publish it. esc.timestamp = hrt_absolute_time(); esc.esc_count = 1; - esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM; + esc.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_PPM; - esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT; + esc.esc[0].esc_vendor = esc_status_s::ESC_VENDOR_GRAUPNER_HOTT; esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10; esc.esc[0].esc_temperature = static_cast(msg.temperature1) - 20.0F; esc.esc[0].esc_voltage = static_cast((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)) * 0.1F; diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 3b8c4ee777..0f957b68ff 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -601,11 +601,11 @@ MK::task_main() esc.counter++; esc.timestamp = hrt_absolute_time(); esc.esc_count = (uint8_t) _num_outputs; - esc.esc_connectiontype = ESC_CONNECTION_TYPE_I2C; + esc.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_I2C; for (unsigned int i = 0; i < _num_outputs; i++) { esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i; - esc.esc[i].esc_vendor = ESC_VENDOR_MIKROKOPTER; + esc.esc[i].esc_vendor = esc_status_s::ESC_VENDOR_MIKROKOPTER; esc.esc[i].esc_version = (uint16_t) Motor[i].Version; esc.esc[i].esc_voltage = 0.0F; esc.esc[i].esc_current = static_cast(Motor[i].Current) * 0.1F; diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h deleted file mode 100644 index 73fe0f9361..0000000000 --- a/src/modules/uORB/topics/esc_status.h +++ /dev/null @@ -1,116 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: @author Marco Bauer - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file esc_status.h - * Definition of the esc_status uORB topic. - * - * Published the state machine and the system status bitfields - * (see SYS_STATUS mavlink message), published only by commander app. - * - * All apps should write to subsystem_info: - * - * (any app) --> subsystem_info (published) --> (commander app state machine) --> esc_status --> (mavlink app) - */ - -#ifndef ESC_STATUS_H_ -#define ESC_STATUS_H_ - -#include -#include -#include "../uORB.h" - -/** - * The number of ESCs supported. - * Current (Q2/2013) we support 8 ESCs, - */ -#define CONNECTED_ESC_MAX 8 - -enum ESC_VENDOR { - ESC_VENDOR_GENERIC = 0, /**< generic ESC */ - ESC_VENDOR_MIKROKOPTER, /**< Mikrokopter */ - ESC_VENDOR_GRAUPNER_HOTT /**< Graupner HoTT ESC */ -}; - -enum ESC_CONNECTION_TYPE { - ESC_CONNECTION_TYPE_PPM = 0, /**< Traditional PPM ESC */ - ESC_CONNECTION_TYPE_SERIAL, /**< Serial Bus connected ESC */ - ESC_CONNECTION_TYPE_ONESHOOT, /**< One Shoot PPM */ - ESC_CONNECTION_TYPE_I2C, /**< I2C */ - ESC_CONNECTION_TYPE_CAN /**< CAN-Bus */ -}; - -/** - * @addtogroup topics - * @{ - */ - -/** - * Electronic speed controller status. - * Unsupported float fields should be assigned NaN. - */ -struct esc_status_s { - /* use of a counter and timestamp recommended (but not necessary) */ - - uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ - uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - - uint8_t esc_count; /**< number of connected ESCs */ - enum ESC_CONNECTION_TYPE esc_connectiontype; /**< how ESCs connected to the system */ - - struct { - enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */ - uint32_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ - int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ - float esc_voltage; /**< Voltage measured from current ESC [V] - if supported */ - float esc_current; /**< Current measured from current ESC [A] - if supported */ - float esc_temperature; /**< Temperature measured from current ESC [degC] - if supported */ - float esc_setpoint; /**< setpoint of current ESC */ - uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */ - uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ - uint16_t esc_version; /**< Version of current ESC - if supported */ - uint16_t esc_state; /**< State of ESC - depend on Vendor */ - } esc[CONNECTED_ESC_MAX]; - -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -//ORB_DECLARE(esc_status); -ORB_DECLARE_OPTIONAL(esc_status); - -#endif diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 51589e43eb..c6719d4817 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -85,7 +85,7 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) { if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) || - (num_outputs > CONNECTED_ESC_MAX)) { + (num_outputs > esc_status_s::CONNECTED_ESC_MAX)) { perf_count(_perfcnt_invalid_input); return; } @@ -159,7 +159,7 @@ void UavcanEscController::arm_single_esc(int num, bool arm) void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) { - if (msg.esc_index < CONNECTED_ESC_MAX) { + if (msg.esc_index < esc_status_s::CONNECTED_ESC_MAX) { _esc_status.esc_count = uavcan::max(_esc_status.esc_count, msg.esc_index + 1); _esc_status.timestamp = msg.getMonotonicTimestamp().toUSec(); @@ -179,7 +179,7 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure< void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) { _esc_status.counter += 1; - _esc_status.esc_connectiontype = ESC_CONNECTION_TYPE_CAN; + _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN; if (_esc_status_pub > 0) { (void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status); From 6ce097546cf9a7195eed1e69b28b9384c41ac484 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 19:15:19 +0200 Subject: [PATCH 42/84] Move fence to generated topics --- msg/fence.msg | 4 ++ msg/fence_vertex.msg | 2 + src/modules/uORB/topics/fence.h | 80 --------------------------------- 3 files changed, 6 insertions(+), 80 deletions(-) create mode 100644 msg/fence.msg create mode 100644 msg/fence_vertex.msg delete mode 100644 src/modules/uORB/topics/fence.h diff --git a/msg/fence.msg b/msg/fence.msg new file mode 100644 index 0000000000..4109a0c8fe --- /dev/null +++ b/msg/fence.msg @@ -0,0 +1,4 @@ +uint8 GEOFENCE_MAX_VERTICES = 16 + +uint32 count # number of actual vertices +fence_vertex[16] vertices # geofence positions diff --git a/msg/fence_vertex.msg b/msg/fence_vertex.msg new file mode 100644 index 0000000000..2e524f9697 --- /dev/null +++ b/msg/fence_vertex.msg @@ -0,0 +1,2 @@ +float32 lat # latitude in degrees, worst case float precision gives us 2 meter resolution at the equator +float32 lon # longitude in degrees, worst case float precision gives us 2 meter resolution at the equator diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h deleted file mode 100644 index 43cee89fe8..0000000000 --- a/src/modules/uORB/topics/fence.h +++ /dev/null @@ -1,80 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Jean Cyr - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file fence.h - * Definition of geofence. - */ - -#ifndef TOPIC_FENCE_H_ -#define TOPIC_FENCE_H_ - -#include -#include -#include - -/** - * @addtogroup topics - * @{ - */ - -#define GEOFENCE_MAX_VERTICES 16 - -/** - * This is the position of a geofence vertex - * - */ -struct fence_vertex_s { - // Worst case float precision gives us 2 meter resolution at the equator - float lat; /**< latitude in degrees */ - float lon; /**< longitude in degrees */ -}; - -/** - * This is the position of a geofence - * - */ -struct fence_s { - unsigned count; /**< number of actual vertices */ - struct fence_vertex_s vertices[GEOFENCE_MAX_VERTICES]; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(fence); - -#endif From f5ce8e4826983fddf6a4736601aaf410f181bdff Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 19:15:38 +0200 Subject: [PATCH 43/84] Update dataman to reflect topic updates --- src/modules/dataman/dataman.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index d625bf9853..ad8df4970b 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -62,7 +62,11 @@ extern "C" { /** The maximum number of instances for each item type */ enum { DM_KEY_SAFE_POINTS_MAX = 8, + #ifdef __cplusplus + DM_KEY_FENCE_POINTS_MAX = fence_s::GEOFENCE_MAX_VERTICES, + #else DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, + #endif DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED, From c9e8af8e9b1fdfd7e7d430db931bd1e3d6347ef8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 19:16:50 +0200 Subject: [PATCH 44/84] Update navigator to reflect topic updates. --- src/modules/navigator/geofence.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 1b66107347..eda0505169 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -259,8 +259,8 @@ Geofence::valid() } // Otherwise - if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) { - warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1); + if ((_verticesCount < 4) || (_verticesCount > fence_s::GEOFENCE_MAX_VERTICES)) { + warnx("Fence must have at least 3 sides and not more than %d", fence_s::GEOFENCE_MAX_VERTICES - 1); return false; } From 20dff14eae9e10b0442d2de9ebb1a8332e70ca97 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 19:17:14 +0200 Subject: [PATCH 45/84] Extend generator templates to allow nested topics --- msg/templates/uorb/msg.h.template | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template index f16095c978..78e1992e06 100644 --- a/msg/templates/uorb/msg.h.template +++ b/msg/templates/uorb/msg.h.template @@ -106,8 +106,10 @@ type_map = {'int8': 'int8_t', 'float32': 'float', 'float64': 'double', 'bool': 'bool', + 'char': 'char', 'fence_vertex': 'fence_vertex', - 'position_setpoint': 'position_setpoint'} + 'position_setpoint': 'position_setpoint', + 'esc_report': 'esc_report'} # Function to print a standard ros type def print_field_def(field): From 01d72adca5f5193f8ec868526c0d3ea091667c50 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 19:20:04 +0200 Subject: [PATCH 46/84] Move debug_key_value to generated topics --- msg/debug_key_value.msg | 3 + src/modules/uORB/topics/debug_key_value.h | 76 ----------------------- 2 files changed, 3 insertions(+), 76 deletions(-) create mode 100644 msg/debug_key_value.msg delete mode 100644 src/modules/uORB/topics/debug_key_value.h diff --git a/msg/debug_key_value.msg b/msg/debug_key_value.msg new file mode 100644 index 0000000000..5b8e187802 --- /dev/null +++ b/msg/debug_key_value.msg @@ -0,0 +1,3 @@ +uint32 timestamp_ms # in milliseconds since system start +int8[10] key # max. 10 characters as key / name +float32 value # the value to send as debug output diff --git a/src/modules/uORB/topics/debug_key_value.h b/src/modules/uORB/topics/debug_key_value.h deleted file mode 100644 index 9253c787db..0000000000 --- a/src/modules/uORB/topics/debug_key_value.h +++ /dev/null @@ -1,76 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file debug_key_value.h - * Definition of the debug named value uORB topic. Allows to send a 10-char key - * with a float as value. - */ - -#ifndef TOPIC_DEBUG_KEY_VALUE_H_ -#define TOPIC_DEBUG_KEY_VALUE_H_ - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * Key/value pair for debugging - */ -struct debug_key_value_s { - - /* - * Actual data, this is specific to the type of data which is stored in this struct - * A line containing L0GME will be added by the Python logging code generator to the - * logged dataset. - */ - uint32_t timestamp_ms; /**< in milliseconds since system start */ - char key[10]; /**< max. 10 characters as key / name */ - float value; /**< the value to send as debug output */ - -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(debug_key_value); - -#endif From 961b6b7802b4fa68e1a2bb8ddd5672fa26bfaea9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 19:21:31 +0200 Subject: [PATCH 47/84] Move vision position estimate to generated topics --- msg/vision_position_estimate.msg | 14 ++++ .../attitude_estimator_ekf_main.cpp | 2 +- .../position_estimator_inav_main.c | 2 +- .../uORB/topics/vision_position_estimate.h | 82 ------------------- 4 files changed, 16 insertions(+), 84 deletions(-) create mode 100644 msg/vision_position_estimate.msg delete mode 100644 src/modules/uORB/topics/vision_position_estimate.h diff --git a/msg/vision_position_estimate.msg b/msg/vision_position_estimate.msg new file mode 100644 index 0000000000..53e2b9c0f0 --- /dev/null +++ b/msg/vision_position_estimate.msg @@ -0,0 +1,14 @@ +uint32 id # ID of the estimator, commonly the component ID of the incoming message + +uint64 timestamp_boot # time of this estimate, in microseconds since system start +uint64 timestamp_computer # timestamp provided by the companion computer, in us + +float32 x # X position in meters in NED earth-fixed frame +float32 y # Y position in meters in NED earth-fixed frame +float32 z # Z position in meters in NED earth-fixed frame (negative altitude) + +float32 vx # X velocity in meters per second in NED earth-fixed frame +float32 vy # Y velocity in meters per second in NED earth-fixed frame +float32 vz # Z velocity in meters per second in NED earth-fixed frame + +float32[4] q # Estimated attitude as quaternion diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index fb7cda8c42..8eb34c49c6 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -290,7 +290,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) math::Matrix<3, 3> R_decl; R_decl.identity(); - struct vision_position_estimate vision {}; + struct vision_position_estimate_s vision {}; /* register the perf counter */ perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index f6d096cec0..7af3a355eb 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -325,7 +325,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&local_pos, 0, sizeof(local_pos)); struct optical_flow_s flow; memset(&flow, 0, sizeof(flow)); - struct vision_position_estimate vision; + struct vision_position_estimate_s vision; memset(&vision, 0, sizeof(vision)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); diff --git a/src/modules/uORB/topics/vision_position_estimate.h b/src/modules/uORB/topics/vision_position_estimate.h deleted file mode 100644 index 74c96cf2e0..0000000000 --- a/src/modules/uORB/topics/vision_position_estimate.h +++ /dev/null @@ -1,82 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vision_position_estimate.h - * Vision based position estimate - */ - -#ifndef TOPIC_VISION_POSITION_ESTIMATE_H_ -#define TOPIC_VISION_POSITION_ESTIMATE_H_ - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * Vision based position estimate in NED frame - */ -struct vision_position_estimate { - - unsigned id; /**< ID of the estimator, commonly the component ID of the incoming message */ - - uint64_t timestamp_boot; /**< time of this estimate, in microseconds since system start */ - uint64_t timestamp_computer; /**< timestamp provided by the companion computer, in us */ - - float x; /**< X position in meters in NED earth-fixed frame */ - float y; /**< Y position in meters in NED earth-fixed frame */ - float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */ - - float vx; /**< X velocity in meters per second in NED earth-fixed frame */ - float vy; /**< Y velocity in meters per second in NED earth-fixed frame */ - float vz; /**< Z velocity in meters per second in NED earth-fixed frame */ - - float q[4]; /**< Estimated attitude as quaternion */ - - // XXX Add covariances here - -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vision_position_estimate); - -#endif /* TOPIC_VISION_POSITION_ESTIMATE_H_ */ From a190d01ade748f06ac595fe6ae2454f6c376c833 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 19:22:26 +0200 Subject: [PATCH 48/84] Move differential pressure to generated topics --- msg/differential_pressure.msg | 6 ++ .../uORB/topics/differential_pressure.h | 71 ------------------- 2 files changed, 6 insertions(+), 71 deletions(-) create mode 100644 msg/differential_pressure.msg delete mode 100644 src/modules/uORB/topics/differential_pressure.h diff --git a/msg/differential_pressure.msg b/msg/differential_pressure.msg new file mode 100644 index 0000000000..bb31449d6c --- /dev/null +++ b/msg/differential_pressure.msg @@ -0,0 +1,6 @@ +uint64 timestamp # Microseconds since system boot, needed to integrate +uint64 error_count # Number of errors detected by driver +float32 differential_pressure_raw_pa # Raw differential pressure reading (may be negative) +float32 differential_pressure_filtered_pa # Low pass filtered differential pressure reading +float32 max_differential_pressure_pa # Maximum differential pressure reading +float32 temperature # Temperature provided by sensor, -1000.0f if unknown diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h deleted file mode 100644 index 7342fcf04f..0000000000 --- a/src/modules/uORB/topics/differential_pressure.h +++ /dev/null @@ -1,71 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2013 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file differential_pressure.h - * - * Definition of differential pressure topic - */ - -#ifndef TOPIC_DIFFERENTIAL_PRESSURE_H_ -#define TOPIC_DIFFERENTIAL_PRESSURE_H_ - -#include "../uORB.h" -#include - -/** - * @addtogroup topics - * @{ - */ - -/** - * Differential pressure. - */ -struct differential_pressure_s { - uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */ - uint64_t error_count; /**< Number of errors detected by driver */ - float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */ - float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ - float max_differential_pressure_pa; /**< Maximum differential pressure reading */ - float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ - -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(differential_pressure); - -#endif From 06ba8d924a116f670efa96a90145b21a80fac0d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 19:23:11 +0200 Subject: [PATCH 49/84] Move estimator_status to generated topics --- msg/estimator_status.msg | 6 ++ .../ekf_att_pos_estimator_main.cpp | 2 +- src/modules/uORB/topics/estimator_status.h | 80 ------------------- 3 files changed, 7 insertions(+), 81 deletions(-) create mode 100644 msg/estimator_status.msg delete mode 100644 src/modules/uORB/topics/estimator_status.h diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg new file mode 100644 index 0000000000..92e5303a6a --- /dev/null +++ b/msg/estimator_status.msg @@ -0,0 +1,6 @@ +uint64 timestamp # Timestamp in microseconds since boot +float32[32] states # Internal filter states +float32 n_states # Number of states effectively used +uint8 nan_flags # Bitmask to indicate NaN states +uint8 health_flags # Bitmask to indicate sensor health states (vel, pos, hgt) +uint8 timeout_flags # Bitmask to indicate timeout flags (vel, pos, hgt) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e0b4cb2a0d..f3fe048e78 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -395,7 +395,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() } } - struct estimator_status_report rep; + struct estimator_status_s rep; memset(&rep, 0, sizeof(rep)); diff --git a/src/modules/uORB/topics/estimator_status.h b/src/modules/uORB/topics/estimator_status.h deleted file mode 100644 index 7f26b505b7..0000000000 --- a/src/modules/uORB/topics/estimator_status.h +++ /dev/null @@ -1,80 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file estimator_status.h - * Definition of the estimator_status_report uORB topic. - * - * @author Lorenz Meier - */ - -#ifndef ESTIMATOR_STATUS_H_ -#define ESTIMATOR_STATUS_H_ - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * Estimator status report. - * - * This is a generic status report struct which allows any of the onboard estimators - * to write the internal state to the system log. - * - */ -struct estimator_status_report { - - /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes - change with consideration only */ - - uint64_t timestamp; /**< Timestamp in microseconds since boot */ - float states[32]; /**< Internal filter states */ - float n_states; /**< Number of states effectively used */ - uint8_t nan_flags; /**< Bitmask to indicate NaN states */ - uint8_t health_flags; /**< Bitmask to indicate sensor health states (vel, pos, hgt) */ - uint8_t timeout_flags; /**< Bitmask to indicate timeout flags (vel, pos, hgt) */ - -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(estimator_status); - -#endif From d400643738f6a3b9d9f245a87d0c837f55daa179 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 19:23:43 +0200 Subject: [PATCH 50/84] Move battery status to generated topics --- msg/battery_status.msg | 5 ++ src/modules/uORB/topics/battery_status.h | 69 ------------------------ 2 files changed, 5 insertions(+), 69 deletions(-) create mode 100644 msg/battery_status.msg delete mode 100644 src/modules/uORB/topics/battery_status.h diff --git a/msg/battery_status.msg b/msg/battery_status.msg new file mode 100644 index 0000000000..4ed56dd038 --- /dev/null +++ b/msg/battery_status.msg @@ -0,0 +1,5 @@ +uint64 timestamp # microseconds since system boot, needed to integrate +float32 voltage_v # Battery voltage in volts, 0 if unknown +float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown +float32 current_a # Battery current in amperes, -1 if unknown +float32 discharged_mah # Discharged amount in mAh, -1 if unknown diff --git a/src/modules/uORB/topics/battery_status.h b/src/modules/uORB/topics/battery_status.h deleted file mode 100644 index d473dff3f4..0000000000 --- a/src/modules/uORB/topics/battery_status.h +++ /dev/null @@ -1,69 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2013 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file battery_status.h - * - * Definition of the battery status uORB topic. - */ - -#ifndef BATTERY_STATUS_H_ -#define BATTERY_STATUS_H_ - -#include "../uORB.h" -#include - -/** - * @addtogroup topics - * @{ - */ - -/** - * Battery voltages and status - */ -struct battery_status_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ - float voltage_v; /**< Battery voltage in volts, 0 if unknown */ - float voltage_filtered_v; /**< Battery voltage in volts, filtered, 0 if unknown */ - float current_a; /**< Battery current in amperes, -1 if unknown */ - float discharged_mah; /**< Discharged amount in mAh, -1 if unknown */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(battery_status); - -#endif From a994e5826705018e394c7c4bbcb598f7fc2b8ff3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 19:24:05 +0200 Subject: [PATCH 51/84] Move encoders to generated topics --- msg/encoders.msg | 5 +++ src/modules/uORB/topics/encoders.h | 66 ------------------------------ 2 files changed, 5 insertions(+), 66 deletions(-) create mode 100644 msg/encoders.msg delete mode 100644 src/modules/uORB/topics/encoders.h diff --git a/msg/encoders.msg b/msg/encoders.msg new file mode 100644 index 0000000000..505365c57e --- /dev/null +++ b/msg/encoders.msg @@ -0,0 +1,5 @@ +uint8 NUM_ENCODERS = 4 + +uint64 timestamp +int64[4] counts # counts of encoder +float32[4] velocity # counts of encoder/ second diff --git a/src/modules/uORB/topics/encoders.h b/src/modules/uORB/topics/encoders.h deleted file mode 100644 index 588c0ddb13..0000000000 --- a/src/modules/uORB/topics/encoders.h +++ /dev/null @@ -1,66 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file encoders.h - * - * Encoders topic. - * - */ - -#ifndef TOPIC_ENCODERS_H -#define TOPIC_ENCODERS_H - -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -#define NUM_ENCODERS 4 - -struct encoders_s { - uint64_t timestamp; - int64_t counts[NUM_ENCODERS]; // counts of encoder - float velocity[NUM_ENCODERS]; // counts of encoder/ second -}; - -/** - * @} - */ - -ORB_DECLARE(encoders); - -#endif From 0e51092eabbec4993036ce88e2c09663b5232b90 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 19:24:40 +0200 Subject: [PATCH 52/84] Move geofence_result to generated topics --- msg/geofence_result.msg | 1 + src/modules/uORB/topics/geofence_result.h | 64 ----------------------- 2 files changed, 1 insertion(+), 64 deletions(-) create mode 100644 msg/geofence_result.msg delete mode 100644 src/modules/uORB/topics/geofence_result.h diff --git a/msg/geofence_result.msg b/msg/geofence_result.msg new file mode 100644 index 0000000000..7fc21c2af8 --- /dev/null +++ b/msg/geofence_result.msg @@ -0,0 +1 @@ +bool geofence_violated # true if the geofence is violated diff --git a/src/modules/uORB/topics/geofence_result.h b/src/modules/uORB/topics/geofence_result.h deleted file mode 100644 index 8d32dfc0ae..0000000000 --- a/src/modules/uORB/topics/geofence_result.h +++ /dev/null @@ -1,64 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2014 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file geofence_result.h - * Status of the plance concerning the geofence - * - * @author Ban Siesta - */ - -#ifndef TOPIC_GEOFENCE_RESULT_H -#define TOPIC_GEOFENCE_RESULT_H - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -struct geofence_result_s { - bool geofence_violated; /**< true if the geofence is violated */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(geofence_result); - -#endif From 2f5e40e6d0622d7642edb5b586c17095f8600aaf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 19:25:00 +0200 Subject: [PATCH 53/84] Update MAVLink app with topic updates --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e0ce617fae..b8fa50f46a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -722,7 +722,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) mavlink_vision_position_estimate_t pos; mavlink_msg_vision_position_estimate_decode(msg, &pos); - struct vision_position_estimate vision_position; + struct vision_position_estimate_s vision_position; memset(&vision_position, 0, sizeof(vision_position)); // Use the component ID to identify the vision sensor From e87dc298113f3feb3a5502070679116b0fef4f63 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 19:25:13 +0200 Subject: [PATCH 54/84] sdlog2: Update with new topic changes --- src/modules/sdlog2/sdlog2.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b88400c276..76fd47cc15 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1069,7 +1069,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; - struct vision_position_estimate vision_pos; + struct vision_position_estimate_s vision_pos; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -1079,7 +1079,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct battery_status_s battery; struct telemetry_status_s telemetry; struct range_finder_report range_finder; - struct estimator_status_report estimator_status; + struct estimator_status_s estimator_status; struct tecs_status_s tecs_status; struct system_power_s system_power; struct servorail_status_s servorail_status; From 7b90746a7fab9b0a591791d298c5a4ffa93c19ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 19:25:30 +0200 Subject: [PATCH 55/84] uORB: Update objects_common.cpp with newly generated topics --- src/modules/uORB/objects_common.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 1a38b981e8..fc7a87b3f0 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -234,10 +234,10 @@ ORB_DEFINE(esc_status, struct esc_status_s); ORB_DEFINE(encoders, struct encoders_s); #include "topics/estimator_status.h" -ORB_DEFINE(estimator_status, struct estimator_status_report); +ORB_DEFINE(estimator_status, struct estimator_status_s); #include "topics/vision_position_estimate.h" -ORB_DEFINE(vision_position_estimate, struct vision_position_estimate); +ORB_DEFINE(vision_position_estimate, struct vision_position_estimate_s); #include "topics/vehicle_force_setpoint.h" ORB_DEFINE(vehicle_force_setpoint, struct vehicle_force_setpoint_s); From 33cf01f09b56de57241b7d73e8377a3b116a0948 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 19:26:44 +0200 Subject: [PATCH 56/84] Remove unused omnidirectional_flow topic --- src/modules/uORB/objects_common.cpp | 3 - .../uORB/topics/omnidirectional_flow.h | 76 ------------------- 2 files changed, 79 deletions(-) delete mode 100644 src/modules/uORB/topics/omnidirectional_flow.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index fc7a87b3f0..87677e0e38 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -172,9 +172,6 @@ ORB_DEFINE(optical_flow, struct optical_flow_s); #include "topics/filtered_bottom_flow.h" ORB_DEFINE(filtered_bottom_flow, struct filtered_bottom_flow_s); -#include "topics/omnidirectional_flow.h" -ORB_DEFINE(omnidirectional_flow, struct omnidirectional_flow_s); - #include "topics/airspeed.h" ORB_DEFINE(airspeed, struct airspeed_s); diff --git a/src/modules/uORB/topics/omnidirectional_flow.h b/src/modules/uORB/topics/omnidirectional_flow.h deleted file mode 100644 index a6ad8a1315..0000000000 --- a/src/modules/uORB/topics/omnidirectional_flow.h +++ /dev/null @@ -1,76 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file optical_flow.h - * Definition of the optical flow uORB topic. - */ - -#ifndef TOPIC_OMNIDIRECTIONAL_FLOW_H_ -#define TOPIC_OMNIDIRECTIONAL_FLOW_H_ - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * Omnidirectional optical flow in NED body frame in SI units. - * - * @see http://en.wikipedia.org/wiki/International_System_of_Units - */ -struct omnidirectional_flow_s { - - uint64_t timestamp; /**< in microseconds since system start */ - - uint16_t left[10]; /**< Left flow, in decipixels */ - uint16_t right[10]; /**< Right flow, in decipixels */ - float front_distance_m; /**< Altitude / distance to object front in meters */ - uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ - uint8_t sensor_id; /**< id of the sensor emitting the flow value */ - -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(omnidirectional_flow); - -#endif From 17108af3e68f2aa9240b188cab2116fc287dafcb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 19:48:48 +0200 Subject: [PATCH 57/84] uORB: Remove unused bodyframe setpoint --- src/modules/uORB/objects_common.cpp | 3 - .../topics/vehicle_bodyframe_speed_setpoint.h | 68 ------------------- 2 files changed, 71 deletions(-) delete mode 100644 src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 87677e0e38..d1c2ef5028 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -130,9 +130,6 @@ ORB_DEFINE(vehicle_control_mode, struct vehicle_control_mode_s); #include "topics/vehicle_local_position_setpoint.h" ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s); -#include "topics/vehicle_bodyframe_speed_setpoint.h" -ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s); - #include "topics/position_setpoint_triplet.h" ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); diff --git a/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h deleted file mode 100644 index fd1ade671a..0000000000 --- a/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h +++ /dev/null @@ -1,68 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vehicle_bodyframe_speed_setpoint.h - * Definition of the bodyframe speed setpoint uORB topic. - */ - -#ifndef TOPIC_VEHICLE_BODYFRAME_SPEED_SETPOINT_H_ -#define TOPIC_VEHICLE_BODYFRAME_SPEED_SETPOINT_H_ - -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -struct vehicle_bodyframe_speed_setpoint_s { - uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - - float vx; /**< in m/s */ - float vy; /**< in m/s */ -// float vz; /**< in m/s */ - float thrust_sp; - float yaw_sp; /**< in radian -PI +PI */ -}; /**< Speed in bodyframe to go to */ - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_bodyframe_speed_setpoint); - -#endif From 44aa18bdd6858a5aebc91dd87b131b4b4262c65e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 May 2015 19:51:50 +0200 Subject: [PATCH 58/84] Remove unused vehicle_control_debug topic --- src/modules/uORB/objects_common.cpp | 3 - .../uORB/topics/vehicle_control_debug.h | 86 ------------------- 2 files changed, 89 deletions(-) delete mode 100644 src/modules/uORB/topics/vehicle_control_debug.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index d1c2ef5028..05e51e73f2 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -157,9 +157,6 @@ ORB_DEFINE(fw_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s); #include "topics/manual_control_setpoint.h" ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); -#include "topics/vehicle_control_debug.h" -ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s); - #include "topics/offboard_control_mode.h" ORB_DEFINE(offboard_control_mode, struct offboard_control_mode_s); diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h deleted file mode 100644 index 2a45eaccc2..0000000000 --- a/src/modules/uORB/topics/vehicle_control_debug.h +++ /dev/null @@ -1,86 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vehicle_control_debug.h - * For debugging purposes to log PID parts of controller - */ - -#ifndef TOPIC_VEHICLE_CONTROL_DEBUG_H_ -#define TOPIC_VEHICLE_CONTROL_DEBUG_H_ - -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ -struct vehicle_control_debug_s { - uint64_t timestamp; /**< in microseconds since system start */ - - float roll_p; /**< roll P control part */ - float roll_i; /**< roll I control part */ - float roll_d; /**< roll D control part */ - - float roll_rate_p; /**< roll rate P control part */ - float roll_rate_i; /**< roll rate I control part */ - float roll_rate_d; /**< roll rate D control part */ - - float pitch_p; /**< pitch P control part */ - float pitch_i; /**< pitch I control part */ - float pitch_d; /**< pitch D control part */ - - float pitch_rate_p; /**< pitch rate P control part */ - float pitch_rate_i; /**< pitch rate I control part */ - float pitch_rate_d; /**< pitch rate D control part */ - - float yaw_p; /**< yaw P control part */ - float yaw_i; /**< yaw I control part */ - float yaw_d; /**< yaw D control part */ - - float yaw_rate_p; /**< yaw rate P control part */ - float yaw_rate_i; /**< yaw rate I control part */ - float yaw_rate_d; /**< yaw rate D control part */ - -}; /**< vehicle_control_debug */ - -/** -* @} -*/ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_control_debug); - -#endif From be6ab2ca7d1d26e9a0a4566bec00eda7e59638f2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:39:24 -0700 Subject: [PATCH 59/84] vehicle_status topic: Changed / fixed signedness --- msg/vehicle_status.msg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 21a1d7a424..07484425b3 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -98,8 +98,8 @@ bool failsafe # true if system is in failsafe state bool calibration_enabled # true if current calibrating parts of the system. Also sets the system to ARMING_STATE_INIT. int32 system_type # system type, inspired by MAVLink's VEHICLE_TYPE enum -int32 system_id # system id, inspired by MAVLink's system ID field -int32 component_id # subsystem / component id, inspired by MAVLink's component ID field +uint32 system_id # system id, inspired by MAVLink's system ID field +uint32 component_id # subsystem / component id, inspired by MAVLink's component ID field bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter bool is_vtol # True if the system is VTOL capable From f7563816d4c1db10846285b84d2c6509b04eeaad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:45:59 -0700 Subject: [PATCH 60/84] uORB: Moved servorail_status to generated topics --- msg/servorail_status.msg | 3 + src/modules/uORB/topics/servorail_status.h | 67 ---------------------- 2 files changed, 3 insertions(+), 67 deletions(-) create mode 100644 msg/servorail_status.msg delete mode 100644 src/modules/uORB/topics/servorail_status.h diff --git a/msg/servorail_status.msg b/msg/servorail_status.msg new file mode 100644 index 0000000000..f473cc1e28 --- /dev/null +++ b/msg/servorail_status.msg @@ -0,0 +1,3 @@ +uint64 timestamp # microseconds since system boot +float32 voltage_v # Servo rail voltage in volts +float32 rssi_v # RSSI pin voltage in volts diff --git a/src/modules/uORB/topics/servorail_status.h b/src/modules/uORB/topics/servorail_status.h deleted file mode 100644 index 55668790b8..0000000000 --- a/src/modules/uORB/topics/servorail_status.h +++ /dev/null @@ -1,67 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2013 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file servorail_status.h - * - * Definition of the servorail status uORB topic. - */ - -#ifndef SERVORAIL_STATUS_H_ -#define SERVORAIL_STATUS_H_ - -#include "../uORB.h" -#include - -/** - * @addtogroup topics - * @{ - */ - -/** - * Servorail voltages and status - */ -struct servorail_status_s { - uint64_t timestamp; /**< microseconds since system boot */ - float voltage_v; /**< Servo rail voltage in volts */ - float rssi_v; /**< RSSI pin voltage in volts */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(servorail_status); - -#endif From f1d038efc07edb336dcf930e199a21e7f214d6b7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:46:32 -0700 Subject: [PATCH 61/84] uORB: Moved subsystem_info to generated topics --- msg/subsystem_info.msg | 22 ++++++ src/modules/uORB/topics/subsystem_info.h | 96 ------------------------ 2 files changed, 22 insertions(+), 96 deletions(-) create mode 100644 msg/subsystem_info.msg delete mode 100644 src/modules/uORB/topics/subsystem_info.h diff --git a/msg/subsystem_info.msg b/msg/subsystem_info.msg new file mode 100644 index 0000000000..bd0b9dd841 --- /dev/null +++ b/msg/subsystem_info.msg @@ -0,0 +1,22 @@ +uint64 SUBSYSTEM_TYPE_GYRO = 1 +uint64 SUBSYSTEM_TYPE_ACC = 2 +uint64 SUBSYSTEM_TYPE_MAG = 4 +uint64 SUBSYSTEM_TYPE_ABSPRESSURE = 8 +uint64 SUBSYSTEM_TYPE_DIFFPRESSURE = 16 +uint64 SUBSYSTEM_TYPE_GPS = 32 +uint64 SUBSYSTEM_TYPE_OPTICALFLOW = 64 +uint64 SUBSYSTEM_TYPE_CVPOSITION = 128 +uint64 SUBSYSTEM_TYPE_LASERPOSITION = 256 +uint64 SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 512 +uint64 SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1024 +uint64 SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 2048 +uint64 SUBSYSTEM_TYPE_YAWPOSITION = 4096 +uint64 SUBSYSTEM_TYPE_ALTITUDECONTROL = 16384 +uint64 SUBSYSTEM_TYPE_POSITIONCONTROL = 32768 +uint64 SUBSYSTEM_TYPE_MOTORCONTROL = 65536 +uint64 SUBSYSTEM_TYPE_RANGEFINDER = 131072 + +bool present +bool enabled +bool ok +uint64 subsystem_type diff --git a/src/modules/uORB/topics/subsystem_info.h b/src/modules/uORB/topics/subsystem_info.h deleted file mode 100644 index d3a7ce10d8..0000000000 --- a/src/modules/uORB/topics/subsystem_info.h +++ /dev/null @@ -1,96 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * Thomas Gubler - * Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file subsystem_info.h - * Definition of the subsystem info topic. - * - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - */ - -#ifndef TOPIC_SUBSYSTEM_INFO_H_ -#define TOPIC_SUBSYSTEM_INFO_H_ - -#include -#include -#include "../uORB.h" - -enum SUBSYSTEM_TYPE { - SUBSYSTEM_TYPE_GYRO = 1, - SUBSYSTEM_TYPE_ACC = 2, - SUBSYSTEM_TYPE_MAG = 4, - SUBSYSTEM_TYPE_ABSPRESSURE = 8, - SUBSYSTEM_TYPE_DIFFPRESSURE = 16, - SUBSYSTEM_TYPE_GPS = 32, - SUBSYSTEM_TYPE_OPTICALFLOW = 64, - SUBSYSTEM_TYPE_CVPOSITION = 128, - SUBSYSTEM_TYPE_LASERPOSITION = 256, - SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 512, - SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1024, - SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 2048, - SUBSYSTEM_TYPE_YAWPOSITION = 4096, - SUBSYSTEM_TYPE_ALTITUDECONTROL = 16384, - SUBSYSTEM_TYPE_POSITIONCONTROL = 32768, - SUBSYSTEM_TYPE_MOTORCONTROL = 65536, - SUBSYSTEM_TYPE_RANGEFINDER = 131072 -}; - -/** - * @addtogroup topics - */ - -/** - * State of individual sub systems - */ -struct subsystem_info_s { - bool present; - bool enabled; - bool ok; - - enum SUBSYSTEM_TYPE subsystem_type; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(subsystem_info); - -#endif /* TOPIC_SUBSYSTEM_INFO_H_ */ - From 520d830cec15866ca1d018bdc51b6133cdb08491 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:47:08 -0700 Subject: [PATCH 62/84] uORB: Moved subsystem_info to generated topics --- msg/system_power.msg | 7 +++ src/modules/uORB/topics/system_power.h | 71 -------------------------- 2 files changed, 7 insertions(+), 71 deletions(-) create mode 100644 msg/system_power.msg delete mode 100644 src/modules/uORB/topics/system_power.h diff --git a/msg/system_power.msg b/msg/system_power.msg new file mode 100644 index 0000000000..cb2a7c0eb0 --- /dev/null +++ b/msg/system_power.msg @@ -0,0 +1,7 @@ +uint64 timestamp # microseconds since system boot +float32 voltage5V_v # peripheral 5V rail voltage +uint8 usb_connected # USB is connected when 1 +uint8 brick_valid # brick power is good when 1 +uint8 servo_valid # servo power is good when 1 +uint8 periph_5V_OC # peripheral overcurrent when 1 +uint8 hipower_5V_OC # hi power peripheral overcurrent when 1 diff --git a/src/modules/uORB/topics/system_power.h b/src/modules/uORB/topics/system_power.h deleted file mode 100644 index 7763b60043..0000000000 --- a/src/modules/uORB/topics/system_power.h +++ /dev/null @@ -1,71 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2013 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file system_power.h - * - * Definition of the system_power voltage and power status uORB topic. - */ - -#ifndef SYSTEM_POWER_H_ -#define SYSTEM_POWER_H_ - -#include "../uORB.h" -#include - -/** - * @addtogroup topics - * @{ - */ - -/** - * voltage and power supply status - */ -struct system_power_s { - uint64_t timestamp; /**< microseconds since system boot */ - float voltage5V_v; /**< peripheral 5V rail voltage */ - uint8_t usb_connected:1; /**< USB is connected when 1 */ - uint8_t brick_valid:1; /**< brick power is good when 1 */ - uint8_t servo_valid:1; /**< servo power is good when 1 */ - uint8_t periph_5V_OC:1; /**< peripheral overcurrent when 1 */ - uint8_t hipower_5V_OC:1; /**< hi power peripheral overcurrent when 1 */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(system_power); - -#endif From b36fe0232f34aa34e703809dd68b6571b7fbe7e2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:47:48 -0700 Subject: [PATCH 63/84] uORB: Moved tecs_status to generated topics --- msg/tecs_status.msg | 26 ++++++++ src/modules/uORB/topics/tecs_status.h | 94 --------------------------- 2 files changed, 26 insertions(+), 94 deletions(-) create mode 100644 msg/tecs_status.msg delete mode 100644 src/modules/uORB/topics/tecs_status.h diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg new file mode 100644 index 0000000000..ccac921e2b --- /dev/null +++ b/msg/tecs_status.msg @@ -0,0 +1,26 @@ +uint8 TECS_MODE_NORMAL = 0 +uint8 TECS_MODE_UNDERSPEED = 1 +uint8 TECS_MODE_TAKEOFF = 2 +uint8 TECS_MODE_LAND = 3 +uint8 TECS_MODE_LAND_THROTTLELIM = 4 +uint8 TECS_MODE_BAD_DESCENT = 5 +uint8 TECS_MODE_CLIMBOUT = 6 + +uint64 timestamp # timestamp, in microseconds since system start */ + +float32 altitudeSp +float32 altitude_filtered +float32 flightPathAngleSp +float32 flightPathAngle +float32 flightPathAngleFiltered +float32 airspeedSp +float32 airspeed_filtered +float32 airspeedDerivativeSp +float32 airspeedDerivative + +float32 totalEnergyRateSp +float32 totalEnergyRate +float32 energyDistributionRateSp +float32 energyDistributionRate + +uint8 mode diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h deleted file mode 100644 index 3c858901c7..0000000000 --- a/src/modules/uORB/topics/tecs_status.h +++ /dev/null @@ -1,94 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vehicle_global_position.h - * Definition of the global fused WGS84 position uORB topic. - * - * @author Thomas Gubler - */ - -#ifndef TECS_STATUS_T_H_ -#define TECS_STATUS_T_H_ - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -typedef enum { - TECS_MODE_NORMAL = 0, - TECS_MODE_UNDERSPEED, - TECS_MODE_TAKEOFF, - TECS_MODE_LAND, - TECS_MODE_LAND_THROTTLELIM, - TECS_MODE_BAD_DESCENT, - TECS_MODE_CLIMBOUT -} tecs_mode; - -/** -* Internal values of the (m)TECS fixed wing speed alnd altitude control system -*/ -struct tecs_status_s { - uint64_t timestamp; /**< timestamp, in microseconds since system start */ - - float altitudeSp; - float altitude_filtered; - float flightPathAngleSp; - float flightPathAngle; - float flightPathAngleFiltered; - float airspeedSp; - float airspeed_filtered; - float airspeedDerivativeSp; - float airspeedDerivative; - - float totalEnergyRateSp; - float totalEnergyRate; - float energyDistributionRateSp; - float energyDistributionRate; - - tecs_mode mode; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(tecs_status); - -#endif From 38c0a1a251d0f8390e5a3462136a2eb3897bb045 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:48:30 -0700 Subject: [PATCH 64/84] uORB: Moved test_motor to generated topics --- msg/test_motor.msg | 5 ++ src/modules/uORB/topics/test_motor.h | 70 ---------------------------- 2 files changed, 5 insertions(+), 70 deletions(-) create mode 100644 msg/test_motor.msg delete mode 100644 src/modules/uORB/topics/test_motor.h diff --git a/msg/test_motor.msg b/msg/test_motor.msg new file mode 100644 index 0000000000..ec06e64fd6 --- /dev/null +++ b/msg/test_motor.msg @@ -0,0 +1,5 @@ +uint8 NUM_MOTOR_OUTPUTS = 8 + +uint64 timestamp # output timestamp in us since system boot +uint32 motor_number # number of motor to spin +float32 value # output power, range [0..1] diff --git a/src/modules/uORB/topics/test_motor.h b/src/modules/uORB/topics/test_motor.h deleted file mode 100644 index 2dd90e69da..0000000000 --- a/src/modules/uORB/topics/test_motor.h +++ /dev/null @@ -1,70 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2014 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file test_motor.h - * - * Test a single drive motor while the vehicle is disarmed - * - * Publishing values to this topic causes a single motor to spin, e.g. for - * ground test purposes. This topic will be ignored while the vehicle is armed. - * - */ - -#ifndef TOPIC_TEST_MOTOR_H -#define TOPIC_TEST_MOTOR_H - -#include -#include "../uORB.h" - -#define NUM_MOTOR_OUTPUTS 8 - -/** - * @addtogroup topics - * @{ - */ - -struct test_motor_s { - uint64_t timestamp; /**< output timestamp in us since system boot */ - unsigned motor_number; /**< number of motor to spin */ - float value; /**< output power, range [0..1] */ -}; - -/** - * @} - */ - -/* actuator output sets; this list can be expanded as more drivers emerge */ -ORB_DECLARE(test_motor); - -#endif From 7fb581291807b28e001991db9b6a1f514fdd1f8a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:48:58 -0700 Subject: [PATCH 65/84] uORB: Moved time_offset topic to generated topics --- msg/time_offset.msg | 1 + src/modules/uORB/topics/time_offset.h | 67 --------------------------- 2 files changed, 1 insertion(+), 67 deletions(-) create mode 100644 msg/time_offset.msg delete mode 100644 src/modules/uORB/topics/time_offset.h diff --git a/msg/time_offset.msg b/msg/time_offset.msg new file mode 100644 index 0000000000..da2d89ab24 --- /dev/null +++ b/msg/time_offset.msg @@ -0,0 +1 @@ +uint64 offset_ns # time offset between companion system and PX4, in nanoseconds diff --git a/src/modules/uORB/topics/time_offset.h b/src/modules/uORB/topics/time_offset.h deleted file mode 100644 index 99e526c765..0000000000 --- a/src/modules/uORB/topics/time_offset.h +++ /dev/null @@ -1,67 +0,0 @@ -/**************************************************************************** - * - * 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file time_offset.h - * Time synchronisation offset - */ - -#ifndef TOPIC_TIME_OFFSET_H_ -#define TOPIC_TIME_OFFSET_H_ - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * Timesync offset for synchronisation with companion computer, GCS, etc. - */ -struct time_offset_s { - - uint64_t offset_ns; /**< time offset between companion system and PX4, in nanoseconds */ - -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(time_offset); - -#endif /* TOPIC_TIME_OFFSET_H_ */ From 5d62fa419a109c1ef56e723707b004d143e59bb3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:49:40 -0700 Subject: [PATCH 66/84] uORB: Moved vehicle_command to generated topics --- msg/vehicle_command.msg | 81 ++++++++++ src/modules/uORB/topics/vehicle_command.h | 172 ---------------------- 2 files changed, 81 insertions(+), 172 deletions(-) create mode 100644 msg/vehicle_command.msg delete mode 100644 src/modules/uORB/topics/vehicle_command.h diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg new file mode 100644 index 0000000000..391dc01aa0 --- /dev/null +++ b/msg/vehicle_command.msg @@ -0,0 +1,81 @@ + +uint32 VEHICLE_CMD_CUSTOM_0 = 0 # test command +uint32 VEHICLE_CMD_CUSTOM_1 = 1 # test command +uint32 VEHICLE_CMD_CUSTOM_2 = 2 # test command +uint32 VEHICLE_CMD_NAV_WAYPOINT = 16 # Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| +uint32 VEHICLE_CMD_NAV_LOITER_UNLIM = 17 # Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| +uint32 VEHICLE_CMD_NAV_LOITER_TURNS = 18 # Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| +uint32 VEHICLE_CMD_NAV_LOITER_TIME = 19 # Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| +uint32 VEHICLE_CMD_NAV_RETURN_TO_LAUNCH = 20 # Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_NAV_LAND = 21 # Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| +uint32 VEHICLE_CMD_NAV_TAKEOFF = 22 # Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| +uint32 VEHICLE_CMD_NAV_ROI = 80 # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| +uint32 VEHICLE_CMD_NAV_PATHPLANNING = 81 # Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| +uint32 VEHICLE_CMD_NAV_GUIDED_LIMITS = 90 # set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| +uint32 VEHICLE_CMD_NAV_GUIDED_MASTER = 91 # set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_NAV_GUIDED_ENABLE = 92 # hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_NAV_LAST = 95 # NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_CONDITION_DELAY = 112 # Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_CONDITION_CHANGE_ALT = 113 # Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| +uint32 VEHICLE_CMD_CONDITION_DISTANCE = 114 # Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_CONDITION_YAW = 115 # Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| +uint32 VEHICLE_CMD_CONDITION_LAST = 159 # NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_SET_MODE = 176 # Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_JUMP = 177 # Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_CHANGE_SPEED = 178 # Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_SET_HOME = 179 # Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| +uint32 VEHICLE_CMD_DO_SET_PARAMETER = 180 # Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_SET_RELAY = 181 # Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_REPEAT_RELAY = 182 # Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_SET_SERVO = 183 # Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_REPEAT_SERVO = 184 # Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_FLIGHTTERMINATION=185 # Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_CONTROL_VIDEO = 200 # Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_MOUNT_CONFIGURE=204 # Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_MOUNT_CONTROL=205 # Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| +uint32 VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST=206 # Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_FENCE_ENABLE=207 # Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_PARACHUTE=208 # Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_INVERTED_FLIGHT=210 # Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT=220 # Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_GUIDED_MASTER=221 # set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_GUIDED_LIMITS=222 # set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_LAST = 240 # NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241 # Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| +uint32 VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242 # Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| +uint32 VEHICLE_CMD_PREFLIGHT_STORAGE = 245 # Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| +uint32 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| +uint32 VEHICLE_CMD_OVERRIDE_GOTO = 252 # Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| +uint32 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| +uint32 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm| +uint32 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| +uint32 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan +uint32 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment + +uint32 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED | +uint32 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED | +uint32 VEHICLE_CMD_RESULT_DENIED = 2 # Command PERMANENTLY DENIED | +uint32 VEHICLE_CMD_RESULT_UNSUPPORTED = 3 # Command UNKNOWN/UNSUPPORTED | +uint32 VEHICLE_CMD_RESULT_FAILED = 4 # Command executed, but failed | +uint32 VEHICLE_CMD_RESULT_ENUM_END = 5 # + +uint32 VEHICLE_MOUNT_MODE_RETRACT = 0 # Load and keep safe position (Roll,Pitch,Yaw) from permant memory and stop stabilization | +uint32 VEHICLE_MOUNT_MODE_NEUTRAL = 1 # Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. | +uint32 VEHICLE_MOUNT_MODE_MAVLINK_TARGETING = 2 # Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | +uint32 VEHICLE_MOUNT_MODE_RC_TARGETING = 3 # Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | +uint32 VEHICLE_MOUNT_MODE_GPS_POINT = 4 # Load neutral position and start to point to Lat,Lon,Alt | +uint32 VEHICLE_MOUNT_MODE_ENUM_END = 5 # + +float32 param1 # Parameter 1, as defined by MAVLink uint32 VEHICLE_CMD enum. +float32 param2 # Parameter 2, as defined by MAVLink uint32 VEHICLE_CMD enum. +float32 param3 # Parameter 3, as defined by MAVLink uint32 VEHICLE_CMD enum. +float32 param4 # Parameter 4, as defined by MAVLink uint32 VEHICLE_CMD enum. +float64 param5 # Parameter 5, as defined by MAVLink uint32 VEHICLE_CMD enum. +float64 param6 # Parameter 6, as defined by MAVLink uint32 VEHICLE_CMD enum. +float32 param7 # Parameter 7, as defined by MAVLink uint32 VEHICLE_CMD enum. +uint32 command # Command ID, as defined MAVLink by uint32 VEHICLE_CMD enum. +uint32 target_system # System which should execute the command +uint32 target_component # Component which should execute the command, 0 for all components +uint32 source_system # System sending the command +uint32 source_component # Component sending the command +uint8 confirmation # 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h deleted file mode 100644 index 35161a3260..0000000000 --- a/src/modules/uORB/topics/vehicle_command.h +++ /dev/null @@ -1,172 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2014 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vehicle_command.h - * Definition of the vehicle command uORB topic. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - * @author Anton Matosov - */ - -#ifndef TOPIC_VEHICLE_COMMAND_H_ -#define TOPIC_VEHICLE_COMMAND_H_ - -#include -#include "../uORB.h" - -/** - * Commands for commander app. - * - * Should contain all commands from MAVLink's VEHICLE_CMD ENUM, - * but can contain additional ones. - */ -enum VEHICLE_CMD { - VEHICLE_CMD_CUSTOM_0 = 0, /* test command */ - VEHICLE_CMD_CUSTOM_1 = 1, /* test command */ - VEHICLE_CMD_CUSTOM_2 = 2, /* test command */ - VEHICLE_CMD_NAV_WAYPOINT = 16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_LOITER_UNLIM = 17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_LOITER_TURNS = 18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_LOITER_TIME = 19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_RETURN_TO_LAUNCH = 20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_NAV_LAND = 21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */ - VEHICLE_CMD_NAV_GUIDED_MASTER=91, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_CONDITION_DELAY = 112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_CONDITION_CHANGE_ALT = 113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ - VEHICLE_CMD_CONDITION_DISTANCE = 114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_CONDITION_YAW = 115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ - VEHICLE_CMD_CONDITION_LAST = 159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_MODE = 176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_JUMP = 177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_CHANGE_SPEED = 178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_HOME = 179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_DO_SET_PARAMETER = 180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_RELAY = 181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_REPEAT_RELAY = 182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_SERVO = 183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_REPEAT_SERVO = 184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_CONTROL_VIDEO = 200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| */ - VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_INVERTED_FLIGHT=210, /* Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_GUIDED_MASTER=221, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_GUIDED_LIMITS=222, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_LAST = 240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ - VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - VEHICLE_CMD_PREFLIGHT_STORAGE = 245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ - VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ - VEHICLE_CMD_OVERRIDE_GOTO = 252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ - VEHICLE_CMD_MISSION_START = 300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ - VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - VEHICLE_CMD_START_RX_PAIR = 500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001, /**< Prepare a payload deployment in the flight plan */ - VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 /**< Control a pre-programmed payload deployment */ -}; - -/** - * Commands for commander app. - * - * Should contain all of MAVLink's MAV_CMD_RESULT values - * but can contain additional ones. - */ -enum VEHICLE_CMD_RESULT { - VEHICLE_CMD_RESULT_ACCEPTED = 0, /* Command ACCEPTED and EXECUTED | */ - VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1, /* Command TEMPORARY REJECTED/DENIED | */ - VEHICLE_CMD_RESULT_DENIED = 2, /* Command PERMANENTLY DENIED | */ - VEHICLE_CMD_RESULT_UNSUPPORTED = 3, /* Command UNKNOWN/UNSUPPORTED | */ - VEHICLE_CMD_RESULT_FAILED = 4, /* Command executed, but failed | */ - VEHICLE_CMD_RESULT_ENUM_END = 5, /* | */ -}; - -/** - * Commands for gimbal app. - * - * Should contain all of MAVLink's MAV_MOUNT_MODE values - * but can contain additional ones. - */ -typedef enum VEHICLE_MOUNT_MODE -{ - VEHICLE_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from permant memory and stop stabilization | */ - VEHICLE_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. | */ - VEHICLE_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */ - VEHICLE_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */ - VEHICLE_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */ - VEHICLE_MOUNT_MODE_ENUM_END=5, /* | */ -} VEHICLE_MOUNT_MODE; - -/** - * @addtogroup topics - * @{ - */ - -struct vehicle_command_s { - float param1; /**< Parameter 1, as defined by MAVLink VEHICLE_CMD enum. */ - float param2; /**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum. */ - float param3; /**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum. */ - float param4; /**< Parameter 4, as defined by MAVLink VEHICLE_CMD enum. */ - double param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ - double param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ - float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */ - enum VEHICLE_CMD command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */ - uint8_t target_system; /**< System which should execute the command */ - uint8_t target_component; /**< Component which should execute the command, 0 for all components */ - uint8_t source_system; /**< System sending the command */ - uint8_t source_component; /**< Component sending the command */ - uint8_t confirmation; /**< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) */ -}; /**< command sent to vehicle */ - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_command); - - - -#endif From 411e0b2f84b5aa006c6ab030dcc9a47890fe7ebc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:50:25 -0700 Subject: [PATCH 67/84] uORB: Moved vehicle_land_detected topic to generated topics --- msg/vehicle_land_detected.msg | 2 + .../uORB/topics/vehicle_land_detected.h | 63 ------------------- 2 files changed, 2 insertions(+), 63 deletions(-) create mode 100644 msg/vehicle_land_detected.msg delete mode 100644 src/modules/uORB/topics/vehicle_land_detected.h diff --git a/msg/vehicle_land_detected.msg b/msg/vehicle_land_detected.msg new file mode 100644 index 0000000000..ce6ea8e9a1 --- /dev/null +++ b/msg/vehicle_land_detected.msg @@ -0,0 +1,2 @@ +uint64 timestamp # timestamp of the setpoint +bool landed # true if vehicle is currently landed on the ground diff --git a/src/modules/uORB/topics/vehicle_land_detected.h b/src/modules/uORB/topics/vehicle_land_detected.h deleted file mode 100644 index 51b3568e82..0000000000 --- a/src/modules/uORB/topics/vehicle_land_detected.h +++ /dev/null @@ -1,63 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vehicle_land_detected.h - * Land detected uORB topic - * - * @author Johan Jansen - */ - -#ifndef __TOPIC_VEHICLE_LANDED_H__ -#define __TOPIC_VEHICLE_LANDED_H__ - -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -struct vehicle_land_detected_s { - uint64_t timestamp; /**< timestamp of the setpoint */ - bool landed; /**< true if vehicle is currently landed on the ground*/ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_land_detected); - -#endif //__TOPIC_VEHICLE_LANDED_H__ From a75c0d8eb1b4aed14e95ae1ce004c8809d0e8fed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:51:01 -0700 Subject: [PATCH 68/84] uORB: Moved vehicle_vicon_position to generated topics --- msg/vehicle_vicon_position.msg | 10 +++ .../uORB/topics/vehicle_vicon_position.h | 77 ------------------- 2 files changed, 10 insertions(+), 77 deletions(-) create mode 100644 msg/vehicle_vicon_position.msg delete mode 100644 src/modules/uORB/topics/vehicle_vicon_position.h diff --git a/msg/vehicle_vicon_position.msg b/msg/vehicle_vicon_position.msg new file mode 100644 index 0000000000..1626d85383 --- /dev/null +++ b/msg/vehicle_vicon_position.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time of this estimate, in microseconds since system start +bool valid # true if position satisfies validity criteria of estimator + +float32 x # X position in meters in NED earth-fixed frame +float32 y # Y position in meters in NED earth-fixed frame +float32 z # Z position in meters in NED earth-fixed frame (negative altitude) +float32 roll +float32 pitch +float32 yaw +float32[4] q # Attitude as quaternion diff --git a/src/modules/uORB/topics/vehicle_vicon_position.h b/src/modules/uORB/topics/vehicle_vicon_position.h deleted file mode 100644 index e19a34a5d7..0000000000 --- a/src/modules/uORB/topics/vehicle_vicon_position.h +++ /dev/null @@ -1,77 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vehicle_vicon_position.h - * Definition of the raw VICON Motion Capture position - */ - -#ifndef TOPIC_VEHICLE_VICON_POSITION_H_ -#define TOPIC_VEHICLE_VICON_POSITION_H_ - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * Fused local position in NED. - */ -struct vehicle_vicon_position_s { - uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - bool valid; /**< true if position satisfies validity criteria of estimator */ - - float x; /**< X positin in meters in NED earth-fixed frame */ - float y; /**< X positin in meters in NED earth-fixed frame */ - float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */ - float roll; - float pitch; - float yaw; - - // TODO Add covariances here - -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_vicon_position); - -#endif From 3da7da9466799a854febed0d57c4dc730b78fbea Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:52:32 -0700 Subject: [PATCH 69/84] uORB: Moved wind_estimate and vtol_vehicle_status topics to generated topics. --- msg/vtol_vehicle_status.msg | 4 ++ msg/wind_estimate.msg | 5 ++ src/modules/uORB/topics/vtol_vehicle_status.h | 68 ------------------- src/modules/uORB/topics/wind_estimate.h | 68 ------------------- 4 files changed, 9 insertions(+), 136 deletions(-) create mode 100644 msg/vtol_vehicle_status.msg create mode 100644 msg/wind_estimate.msg delete mode 100644 src/modules/uORB/topics/vtol_vehicle_status.h delete mode 100644 src/modules/uORB/topics/wind_estimate.h diff --git a/msg/vtol_vehicle_status.msg b/msg/vtol_vehicle_status.msg new file mode 100644 index 0000000000..5b2dc991c9 --- /dev/null +++ b/msg/vtol_vehicle_status.msg @@ -0,0 +1,4 @@ +uint64 timestamp # Microseconds since system boot +bool vtol_in_rw_mode # true: vtol vehicle is in rotating wing mode +bool fw_permanent_stab # In fw mode stabilize attitude even if in manual mode +float32 airspeed_tot # Estimated airspeed over control surfaces diff --git a/msg/wind_estimate.msg b/msg/wind_estimate.msg new file mode 100644 index 0000000000..fbd7b638b8 --- /dev/null +++ b/msg/wind_estimate.msg @@ -0,0 +1,5 @@ +uint64 timestamp # Microseconds since system boot +float32 windspeed_north # Wind component in north / X direction +float32 windspeed_east # Wind component in east / Y direction +float32 covariance_north # Uncertainty - set to zero (no uncertainty) if not estimated +float32 covariance_east # Uncertainty - set to zero (no uncertainty) if not estimated diff --git a/src/modules/uORB/topics/vtol_vehicle_status.h b/src/modules/uORB/topics/vtol_vehicle_status.h deleted file mode 100644 index 968c2b6bdf..0000000000 --- a/src/modules/uORB/topics/vtol_vehicle_status.h +++ /dev/null @@ -1,68 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vtol_status.h - * - * Vtol status topic - * - */ - -#ifndef TOPIC_VTOL_STATUS_H -#define TOPIC_VTOL_STATUS_H - -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/* Indicates in which mode the vtol aircraft is in */ -struct vtol_vehicle_status_s { - - uint64_t timestamp; /**< Microseconds since system boot */ - bool vtol_in_rw_mode; /*true: vtol vehicle is in rotating wing mode */ - bool fw_permanent_stab; /**< In fw mode stabilize attitude even if in manual mode*/ - float airspeed_tot; /*< Estimated airspeed over control surfaces */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vtol_vehicle_status); - -#endif diff --git a/src/modules/uORB/topics/wind_estimate.h b/src/modules/uORB/topics/wind_estimate.h deleted file mode 100644 index 58333a64f8..0000000000 --- a/src/modules/uORB/topics/wind_estimate.h +++ /dev/null @@ -1,68 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file wind_estimate.h - * - * Wind estimate topic topic - * - */ - -#ifndef TOPIC_WIND_ESTIMATE_H -#define TOPIC_WIND_ESTIMATE_H - -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** Wind estimate */ -struct wind_estimate_s { - - uint64_t timestamp; /**< Microseconds since system boot */ - float windspeed_north; /**< Wind component in north / X direction */ - float windspeed_east; /**< Wind component in east / Y direction */ - float covariance_north; /**< Uncertainty - set to zero (no uncertainty) if not estimated */ - float covariance_east; /**< Uncertainty - set to zero (no uncertainty) if not estimated */ -}; - -/** - * @} - */ - -ORB_DECLARE(wind_estimate); - -#endif \ No newline at end of file From aa531aec642f777dd5ea1c38c02f1938e8834c6f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:53:09 -0700 Subject: [PATCH 70/84] airspeed driver: Update topic names --- src/drivers/airspeed/airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index d5935dfd46..b1679f7c57 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -364,7 +364,7 @@ Airspeed::update_status() true, true, _sensor_ok, - SUBSYSTEM_TYPE_DIFFPRESSURE + subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE }; if (_subsys_pub > 0) { From ffc6bb7e85ce7b55e21ab2d0ecbd1c5211e19e32 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:53:23 -0700 Subject: [PATCH 71/84] gimbal driver: Update topic names --- src/drivers/gimbal/gimbal.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index ae75d3a14a..faaff93479 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/src/drivers/gimbal/gimbal.cpp @@ -331,13 +331,13 @@ Gimbal::cycle() orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); - if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL - || cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) { + if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL + || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) { _control_cmd = cmd; _control_cmd_set = true; - } else if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONFIGURE) { + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) { _config_cmd = cmd; _config_cmd_set = true; @@ -358,10 +358,11 @@ Gimbal::cycle() if (_control_cmd_set) { - VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)_control_cmd.param7; + unsigned mountMode = _control_cmd.param7; debug("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2); - if (_control_cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL && + mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { /* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */ roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1; pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2; @@ -370,7 +371,8 @@ Gimbal::cycle() updated = true; } - if (_control_cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && + mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4}; math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler(); From 041a963ca1338567562eac4c291040c7b9e143d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:53:41 -0700 Subject: [PATCH 72/84] ll40s driver: Update topic names --- src/drivers/ll40ls/ll40ls.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 8e2caf8a04..7dc7f26ee2 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -734,7 +734,7 @@ LL40LS::start() true, true, true, - SUBSYSTEM_TYPE_RANGEFINDER + subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER }; static orb_advert_t pub = -1; From 7bfeb1f5ace86d849d446a11c89e03f17e5f877a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:53:54 -0700 Subject: [PATCH 73/84] mb12xx: Update topic names --- src/drivers/mb12xx/mb12xx.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index d5ce5d0feb..e252825fb3 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -647,7 +647,7 @@ MB12XX::start() true, true, true, - SUBSYSTEM_TYPE_RANGEFINDER + subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER }; static orb_advert_t pub = -1; From f46cbae012c4b1f576e1b0786607933c59d43987 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:54:15 -0700 Subject: [PATCH 74/84] px4flow driver: update topic names --- src/drivers/px4flow/px4flow.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index ac20f18327..2166258c15 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -528,7 +528,7 @@ PX4FLOW::start() true, true, true, - SUBSYSTEM_TYPE_OPTICALFLOW + subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW }; static orb_advert_t pub = -1; From ad251d558f707bd48c91fcfce34490cc39c8404c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:54:27 -0700 Subject: [PATCH 75/84] px4io driver: update command names --- src/drivers/px4io/px4io.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index bb2f85d61e..df4064a731 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -768,7 +768,7 @@ PX4IO::init() cmd.param5 = 0; cmd.param6 = 0; cmd.param7 = 0; - cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM; + cmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM; /* ask to confirm command */ cmd.confirmation = 1; @@ -1009,7 +1009,7 @@ PX4IO::task_main() orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); // Check for a DSM pairing command - if (((int)cmd.command == VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) { + if (((int)cmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) { dsm_bind_ioctl((int)cmd.param2); } } From 2d156b39a094a7d5fd93ee789e0f479cad2efbd4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:54:43 -0700 Subject: [PATCH 76/84] trone driver: Update topic names --- src/drivers/trone/trone.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index 6dad7b3b15..deb89f2f65 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -612,7 +612,7 @@ TRONE::start() true, true, true, - SUBSYSTEM_TYPE_RANGEFINDER + subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER }; static orb_advert_t pub = -1; From c5bf6765d2905d50ff7b44d412dfa79549fcacf1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:55:01 -0700 Subject: [PATCH 77/84] bottle_drop: Update command names --- src/modules/bottle_drop/bottle_drop.cpp | 26 ++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index b267209fe3..30abc84095 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -152,7 +152,7 @@ private: void handle_command(struct vehicle_command_s *cmd); - void answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result); + void answer_command(struct vehicle_command_s *cmd, unsigned result); /** * Set the actuators @@ -725,7 +725,7 @@ void BottleDrop::handle_command(struct vehicle_command_s *cmd) { switch (cmd->command) { - case VEHICLE_CMD_CUSTOM_0: + case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: /* * param1 and param2 set to 1: open and drop * param1 set to 1: open @@ -746,10 +746,10 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) mavlink_log_critical(_mavlink_fd, "closing bay"); } - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); break; - case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: + case vehicle_command_s::VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: switch ((int)(cmd->param1 + 0.5f)) { case 0: @@ -777,10 +777,10 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat, (double)_target_position.lon, (double)_target_position.alt); map_projection_init(&ref, _target_position.lat, _target_position.lon); - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); break; - case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: + case vehicle_command_s::VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: if (cmd->param1 < 0) { @@ -813,7 +813,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) } } - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); break; default: @@ -822,25 +822,25 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) } void -BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result) +BottleDrop::answer_command(struct vehicle_command_s *cmd, unsigned result) { switch (result) { - case VEHICLE_CMD_RESULT_ACCEPTED: + case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED: break; - case VEHICLE_CMD_RESULT_DENIED: + case vehicle_command_s::VEHICLE_CMD_RESULT_DENIED: mavlink_log_critical(_mavlink_fd, "command denied: %u", cmd->command); break; - case VEHICLE_CMD_RESULT_FAILED: + case vehicle_command_s::VEHICLE_CMD_RESULT_FAILED: mavlink_log_critical(_mavlink_fd, "command failed: %u", cmd->command); break; - case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + case vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: mavlink_log_critical(_mavlink_fd, "command temporarily rejected: %u", cmd->command); break; - case VEHICLE_CMD_RESULT_UNSUPPORTED: + case vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED: mavlink_log_critical(_mavlink_fd, "command unsupported: %u", cmd->command); break; From c18210b1631b03ad9179f075200b9742d60b50da Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:55:14 -0700 Subject: [PATCH 78/84] commander: Update command names --- .../commander/calibration_routines.cpp | 12 +- src/modules/commander/commander.cpp | 146 +++++++++--------- 2 files changed, 79 insertions(+), 79 deletions(-) diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index e854c9aa7e..e9f83775d7 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -508,14 +508,14 @@ 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) +static void calibrate_answer_command(int mavlink_fd, struct vehicle_command_s &cmd, unsigned result) { switch (result) { - case VEHICLE_CMD_RESULT_ACCEPTED: + case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED: tune_positive(true); break; - case VEHICLE_CMD_RESULT_DENIED: + case vehicle_command_s::VEHICLE_CMD_RESULT_DENIED: mavlink_log_critical(mavlink_fd, "command denied during calibration: %u", cmd.command); tune_negative(true); break; @@ -537,18 +537,18 @@ bool calibrate_cancel_check(int mavlink_fd, int cancel_sub) orb_copy(ORB_ID(vehicle_command), cancel_sub, &cmd); - if (cmd.command == VEHICLE_CMD_PREFLIGHT_CALIBRATION && + if (cmd.command == vehicle_command_s::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); + calibrate_answer_command(mavlink_fd, cmd, vehicle_command_s::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); + calibrate_answer_command(mavlink_fd, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b81c9c1d30..7faf3149f7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -247,7 +247,7 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s & */ void *commander_low_prio_loop(void *arg); -void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result); +void answer_command(struct vehicle_command_s &cmd, unsigned result); int commander_main(int argc, char *argv[]) @@ -456,11 +456,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } /* result of the command */ - enum VEHICLE_CMD_RESULT cmd_result = VEHICLE_CMD_RESULT_UNSUPPORTED; + unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED; /* request to set different system mode */ switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: { + case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t)cmd->param1; uint8_t custom_main_mode = (uint8_t)cmd->param2; @@ -521,15 +521,15 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } if (hil_ret != TRANSITION_DENIED && arming_ret != TRANSITION_DENIED && main_ret != TRANSITION_DENIED) { - cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { - cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } break; - case VEHICLE_CMD_COMPONENT_ARM_DISARM: { + case vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM: { // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints // for logic state parameters @@ -549,7 +549,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s // 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; + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; break; } @@ -559,16 +559,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s if (arming_res == TRANSITION_DENIED) { mavlink_log_critical(mavlink_fd, "REJECTING component arm cmd"); - cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } else { - cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } } } break; - case VEHICLE_CMD_OVERRIDE_GOTO: { + case vehicle_command_s::VEHICLE_CMD_OVERRIDE_GOTO: { // TODO listen vehicle_command topic directly from navigator (?) // Increase by 0.5f and rely on the integer cast @@ -579,12 +579,12 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s if (mav_goto == 0) { // MAV_GOTO_DO_HOLD status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; mavlink_log_critical(mavlink_fd, "Pause mission cmd"); - cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; mavlink_log_critical(mavlink_fd, "Continue mission cmd"); - cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f", @@ -600,7 +600,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s break; /* Flight termination */ - case VEHICLE_CMD_DO_FLIGHTTERMINATION: { + case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: { if (cmd->param1 > 0.5f) { //XXX update state machine? armed_local->force_failsafe = true; @@ -642,11 +642,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s warnx("rc loss mode commanded"); } - cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } break; - case VEHICLE_CMD_DO_SET_HOME: { + case vehicle_command_s::VEHICLE_CMD_DO_SET_HOME: { bool use_current = cmd->param1 > 0.5f; if (use_current) { @@ -658,10 +658,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s home->timestamp = hrt_absolute_time(); - cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { - cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } else { @@ -672,10 +672,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s home->timestamp = hrt_absolute_time(); - cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } - if (cmd_result == VEHICLE_CMD_RESULT_ACCEPTED) { + if (cmd_result == vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED) { warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); @@ -693,16 +693,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } break; - case VEHICLE_CMD_NAV_GUIDED_ENABLE: { + case vehicle_command_s::VEHICLE_CMD_NAV_GUIDED_ENABLE: { transition_result_t res = TRANSITION_DENIED; - static main_state_t main_state_pre_offboard =vehicle_status_s::MAIN_STATE_MANUAL; + static main_state_t main_state_pre_offboard = vehicle_status_s::MAIN_STATE_MANUAL; - if (status_local->main_state !=vehicle_status_s::MAIN_STATE_OFFBOARD) { + if (status_local->main_state != vehicle_status_s::MAIN_STATE_OFFBOARD) { main_state_pre_offboard = status_local->main_state; } if (cmd->param1 > 0.5f) { - res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); + res = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); @@ -722,35 +722,35 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } break; - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: - case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: - case VEHICLE_CMD_PREFLIGHT_STORAGE: - case VEHICLE_CMD_CUSTOM_0: - case VEHICLE_CMD_CUSTOM_1: - case VEHICLE_CMD_CUSTOM_2: - case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: - case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: - case VEHICLE_CMD_DO_MOUNT_CONTROL: - case VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT: - case VEHICLE_CMD_DO_MOUNT_CONFIGURE: + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: + case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: + case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: + case vehicle_command_s::VEHICLE_CMD_CUSTOM_2: + case vehicle_command_s::VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: + case vehicle_command_s::VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: + case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL: + case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT: + case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE: /* ignore commands that handled in low prio loop */ break; default: /* Warn about unsupported commands, this makes sense because only commands * to this component ID (or all) are passed by mavlink. */ - answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); + answer_command(*cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED); break; } - if (cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) { /* already warned about unsupported commands in "default" case */ answer_command(*cmd, cmd_result); } /* send any requested ACKs */ - if (cmd->confirmation > 0 && cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + if (cmd->confirmation > 0 && cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) { /* send acknowledge command */ // XXX TODO } @@ -2566,30 +2566,30 @@ print_reject_arm(const char *msg) } } -void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result) +void answer_command(struct vehicle_command_s &cmd, unsigned result) { switch (result) { - case VEHICLE_CMD_RESULT_ACCEPTED: + case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED: tune_positive(true); break; - case VEHICLE_CMD_RESULT_DENIED: + case vehicle_command_s::VEHICLE_CMD_RESULT_DENIED: mavlink_log_critical(mavlink_fd, "command denied: %u", cmd.command); tune_negative(true); break; - case VEHICLE_CMD_RESULT_FAILED: + case vehicle_command_s::VEHICLE_CMD_RESULT_FAILED: mavlink_log_critical(mavlink_fd, "command failed: %u", cmd.command); tune_negative(true); break; - case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + case vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: /* this needs additional hints to the user - so let other messages pass and be spoken */ mavlink_log_critical(mavlink_fd, "command temporarily rejected: %u", cmd.command); tune_negative(true); break; - case VEHICLE_CMD_RESULT_UNSUPPORTED: + case vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED: mavlink_log_critical(mavlink_fd, "command unsupported: %u", cmd.command); tune_negative(true); break; @@ -2653,49 +2653,49 @@ void *commander_low_prio_loop(void *arg) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* ignore commands the high-prio loop handles */ - if (cmd.command == VEHICLE_CMD_DO_SET_MODE || - cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || - cmd.command == VEHICLE_CMD_NAV_TAKEOFF || - cmd.command == VEHICLE_CMD_DO_SET_SERVO) { + if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_MODE || + cmd.command == vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM || + cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF || + cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_SERVO) { continue; } /* only handle low-priority commands here */ switch (cmd.command) { - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (is_safe(&status, &safety, &armed)) { if (((int)(cmd.param1)) == 1) { - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); usleep(100000); /* reboot */ systemreset(false); } else if (((int)(cmd.param1)) == 3) { - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); usleep(100000); /* reboot to bootloader */ systemreset(true); } else { - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); } } else { - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); } break; - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: { int calib_ret = ERROR; /* try to go to INIT/PREFLIGHT arming state */ if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed, false /* fRunPreArmChecks */, mavlink_fd)) { - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); break; } else { status.calibration_enabled = true; @@ -2703,21 +2703,21 @@ void *commander_low_prio_loop(void *arg) if ((int)(cmd.param1) == 1) { /* gyro calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_gyro_calibration(mavlink_fd); } else if ((int)(cmd.param2) == 1) { /* magnetometer calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_mag_calibration(mavlink_fd); } else if ((int)(cmd.param3) == 1) { /* zero-altitude pressure calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); } else if ((int)(cmd.param4) == 1) { /* RC calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); /* disable RC control input completely */ status.rc_input_blocked = true; calib_ret = OK; @@ -2725,31 +2725,31 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param4) == 2) { /* RC trim calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_trim_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 1) { /* accelerometer calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_accel_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 2) { // board offset calibration - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_level_calibration(mavlink_fd); } else if ((int)(cmd.param6) == 1) { /* airspeed calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_airspeed_calibration(mavlink_fd); } else if ((int)(cmd.param7) == 1) { /* do esc calibration */ - answer_command(cmd,VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_esc_calibration(mavlink_fd, &armed); } else if ((int)(cmd.param4) == 0) { /* RC calibration ended - have we been in one worth confirming? */ if (status.rc_input_blocked) { - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); /* enable RC control input */ status.rc_input_blocked = false; mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN"); @@ -2788,14 +2788,14 @@ void *commander_low_prio_loop(void *arg) break; } - case VEHICLE_CMD_PREFLIGHT_STORAGE: { + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: { if (((int)(cmd.param1)) == 0) { int ret = param_load_default(); if (ret == OK) { mavlink_log_info(mavlink_fd, "settings loaded"); - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); } else { mavlink_log_critical(mavlink_fd, "settings load ERROR"); @@ -2809,7 +2809,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); } - answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); } } else if (((int)(cmd.param1)) == 1) { @@ -2823,7 +2823,7 @@ void *commander_low_prio_loop(void *arg) } /* do not spam MAVLink, but provide the answer / green led mechanism */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); } else { mavlink_log_critical(mavlink_fd, "settings save error"); @@ -2837,14 +2837,14 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); } - answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); } } break; } - case VEHICLE_CMD_START_RX_PAIR: + case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR: /* handled in the IO driver */ break; @@ -2854,8 +2854,8 @@ void *commander_low_prio_loop(void *arg) } /* send any requested ACKs */ - if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE - && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { + if (cmd.confirmation > 0 && cmd.command != vehicle_command_s::VEHICLE_CMD_DO_SET_MODE + && cmd.command != vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM) { /* send acknowledge command */ // XXX TODO } From 17adf552ef44b6e265c4a13f026e0a688d14b38e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:55:32 -0700 Subject: [PATCH 79/84] TECS: Update logging struct --- .../fw_pos_control_l1_main.cpp | 20 +++++++++---------- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 18 ++++++++--------- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 6 +++--- 3 files changed, 22 insertions(+), 22 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 34265d6a4e..a50fc53bf7 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -399,7 +399,7 @@ private: bool climbout_mode, float climbout_pitch_min_rad, float altitude, const math::Vector<3> &ground_speed, - tecs_mode mode = TECS_MODE_NORMAL, + unsigned mode = tecs_status_s::TECS_MODE_NORMAL, bool pitch_max_special = false); }; @@ -1093,7 +1093,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi 0.0f, throttle_max, throttle_land, false, land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed, - land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND); + land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); if (!land_noreturn_vertical) { mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); @@ -1198,7 +1198,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(10.0f)), _global_pos.alt, ground_speed, - TECS_MODE_TAKEOFF, + tecs_status_s::TECS_MODE_TAKEOFF, takeoff_pitch_max_deg != _parameters.pitch_limit_max); /* limit roll motion to ensure enough lift */ @@ -1303,7 +1303,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed, - TECS_MODE_NORMAL); + tecs_status_s::TECS_MODE_NORMAL); } else { _control_mode_current = FW_POSCTRL_MODE_OTHER; @@ -1521,7 +1521,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ bool climbout_mode, float climbout_pitch_min_rad, float altitude, const math::Vector<3> &ground_speed, - tecs_mode mode, bool pitch_max_special) + unsigned mode, bool pitch_max_special) { if (_mTecs.getEnabled()) { /* Using mtecs library: prepare arguments for mtecs call */ @@ -1559,7 +1559,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ } /* No underspeed protection in landing mode */ - _tecs.set_detect_underspeed_enabled(!(mode == TECS_MODE_LAND || mode == TECS_MODE_LAND_THROTTLELIM)); + _tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, @@ -1577,16 +1577,16 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ switch (s.mode) { case TECS::ECL_TECS_MODE_NORMAL: - t.mode = TECS_MODE_NORMAL; + t.mode = tecs_status_s::TECS_MODE_NORMAL; break; case TECS::ECL_TECS_MODE_UNDERSPEED: - t.mode = TECS_MODE_UNDERSPEED; + t.mode = tecs_status_s::TECS_MODE_UNDERSPEED; break; case TECS::ECL_TECS_MODE_BAD_DESCENT: - t.mode = TECS_MODE_BAD_DESCENT; + t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT; break; case TECS::ECL_TECS_MODE_CLIMBOUT: - t.mode = TECS_MODE_CLIMBOUT; + t.mode = tecs_status_s::TECS_MODE_CLIMBOUT; break; } diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index ecdab29366..a855e4092f 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -83,7 +83,7 @@ mTecs::~mTecs() } int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, - float airspeedSp, tecs_mode mode, LimitOverride limitOverride) + float airspeedSp, unsigned mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ if (!isfinite(flightPathAngle) || !isfinite(altitude) || @@ -118,7 +118,7 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti } int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, - float airspeedSp, tecs_mode mode, LimitOverride limitOverride) + float airspeedSp, unsigned mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || @@ -154,7 +154,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng } int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered, - float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride) + float accelerationLongitudinalSp, unsigned mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || @@ -200,23 +200,23 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight } /* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */ - if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) { - mode = TECS_MODE_UNDERSPEED; + if (mode != tecs_status_s::TECS_MODE_LAND && mode != tecs_status_s::TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) { + mode = tecs_status_s::TECS_MODE_UNDERSPEED; } /* Set special ouput limiters if we are not in TECS_MODE_NORMAL */ BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter(); BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter(); - if (mode == TECS_MODE_TAKEOFF) { + if (mode == tecs_status_s::TECS_MODE_TAKEOFF) { outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; - } else if (mode == TECS_MODE_LAND) { + } else if (mode == tecs_status_s::TECS_MODE_LAND) { // only limit pitch but do not limit throttle outputLimiterPitch = &_BlockOutputLimiterLandPitch; - } else if (mode == TECS_MODE_LAND_THROTTLELIM) { + } else if (mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM) { outputLimiterThrottle = &_BlockOutputLimiterLandThrottle; outputLimiterPitch = &_BlockOutputLimiterLandPitch; - } else if (mode == TECS_MODE_UNDERSPEED) { + } else if (mode == tecs_status_s::TECS_MODE_UNDERSPEED) { outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle; outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch; } diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 4c2db0c64d..e114cc3ae0 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -65,19 +65,19 @@ public: * Control in altitude setpoint and speed mode */ int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, - float airspeedSp, tecs_mode mode, LimitOverride limitOverride); + float airspeedSp, unsigned mode, LimitOverride limitOverride); /* * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode */ int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, - float airspeedSp, tecs_mode mode, LimitOverride limitOverride); + float airspeedSp, unsigned mode, LimitOverride limitOverride); /* * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case) */ int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered, - float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride); + float accelerationLongitudinalSp, unsigned mode, LimitOverride limitOverride); /* * Reset all integrators From 1c6127b192c12cd04b4cddd48e975302bd365faf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 May 2015 22:55:47 -0700 Subject: [PATCH 80/84] MAVLink app: Update command struct --- src/modules/mavlink/mavlink_receiver.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b8fa50f46a..efdd457689 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -302,7 +302,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) vcmd.param6 = cmd_mavlink.param6; vcmd.param7 = cmd_mavlink.param7; // XXX do proper translation - vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; + vcmd.command = cmd_mavlink.command; vcmd.target_system = cmd_mavlink.target_system; vcmd.target_component = cmd_mavlink.target_component; vcmd.source_system = msg->sysid; @@ -359,7 +359,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) vcmd.param6 = ((double)cmd_mavlink.y) / 1e7; vcmd.param7 = cmd_mavlink.z; // XXX do proper translation - vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; + vcmd.command = cmd_mavlink.command; vcmd.target_system = cmd_mavlink.target_system; vcmd.target_component = cmd_mavlink.target_component; vcmd.source_system = msg->sysid; @@ -481,7 +481,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) vcmd.param5 = 0; vcmd.param6 = 0; vcmd.param7 = 0; - vcmd.command = VEHICLE_CMD_DO_SET_MODE; + vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; vcmd.target_system = new_mode.target_system; vcmd.target_component = MAV_COMP_ID_ALL; vcmd.source_system = msg->sysid; From d7547d388fdc60c74b838987a99f81d19ab1a8c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 May 2015 15:20:38 -0700 Subject: [PATCH 81/84] Remove auto-generated airspeed topic --- src/modules/uORB/topics/airspeed.h | 72 ------------------------------ 1 file changed, 72 deletions(-) delete mode 100644 src/modules/uORB/topics/airspeed.h diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h deleted file mode 100644 index df4b315146..0000000000 --- a/src/modules/uORB/topics/airspeed.h +++ /dev/null @@ -1,72 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file /Users/lorenzmeier/src/Firmware/msg/airspeed.msg */ - - -#pragma once - -#include -#include - - -#ifndef __cplusplus - -#endif - -/** - * @addtogroup topics - * @{ - */ - - -#ifdef __cplusplus -struct __EXPORT airspeed_s { -#else -struct airspeed_s { -#endif - uint64_t timestamp; - float indicated_airspeed_m_s; - float true_airspeed_m_s; - float air_temperature_celsius; -#ifdef __cplusplus - -#endif -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(airspeed); From 5ac5fae020dcea70898b31159a1b5996ea03562b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 May 2015 11:48:56 -0700 Subject: [PATCH 82/84] MAVLink app: better yaw scaling --- src/modules/mavlink/mavlink_receiver.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index efdd457689..e91e834231 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -937,9 +937,25 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.rc_ppm_frame_length = 0; rc.input_source = RC_INPUT_SOURCE_MAVLINK; rc.rssi = RC_INPUT_RSSI_MAX; + + /* roll */ rc.values[0] = man.x / 2 + 1500; + /* pitch */ rc.values[1] = man.y / 2 + 1500; - rc.values[2] = man.r / 2 + 1500; + + /* + * yaw needs special handling as some joysticks have a circular mechanical mask, + * which makes the corner positions unreachable. + * scale yaw up and clip it to overcome this. + */ + rc.values[2] = man.r / 1.5f + 1500; + if (rc.values[2] > 2000) { + rc.values[2] = 2000; + } else if (rc.values[2] < 1000) { + rc.values[2] = 1000; + } + + /* throttle */ rc.values[3] = man.z + 1000; rc.values[4] = decode_switch_pos_n(man.buttons, 0) * 1000 + 1000; From aab379cde940112f90a26dc4e4bd01a0d6fd08bc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 May 2015 17:41:03 -0700 Subject: [PATCH 83/84] MAVLink app: Fix yaw scaling for joystick input --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e91e834231..8ebe0c696d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -948,7 +948,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) * which makes the corner positions unreachable. * scale yaw up and clip it to overcome this. */ - rc.values[2] = man.r / 1.5f + 1500; + rc.values[2] = man.r / 1.2f + 1500; if (rc.values[2] > 2000) { rc.values[2] = 2000; } else if (rc.values[2] < 1000) { From 46920cfd27fba27e558351995f94b341fc545fc4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 May 2015 17:41:26 -0700 Subject: [PATCH 84/84] GPS driver: Obey non-publish flag in all modes --- src/drivers/gps/gps.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index e3706cf671..30fbdfd8d2 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -361,11 +361,14 @@ GPS::task_main() _report_gps_pos.timestamp_variance = hrt_absolute_time(); _report_gps_pos.timestamp_velocity = hrt_absolute_time(); _report_gps_pos.timestamp_time = hrt_absolute_time(); - if (_report_gps_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); - } else { - _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + if (!(_pub_blocked)) { + if (_report_gps_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } } // GPS is obviously detected successfully, reset statistics