diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-06-07 10:14:16 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-06-07 10:14:16 +0200 |
commit | b5e70927d73f6f2f3e17330c9817e6a5d4e1fc70 (patch) | |
tree | 7472ad121809cb12f2e89fc712787a0edad73364 /src/modules | |
parent | 241837c9df04faaf4b741875cf6b47a22819f010 (diff) | |
parent | c4e2232078f2f28cbf97b8280f40fd988a5c2a75 (diff) | |
download | px4-firmware-b5e70927d73f6f2f3e17330c9817e6a5d4e1fc70.tar.gz px4-firmware-b5e70927d73f6f2f3e17330c9817e6a5d4e1fc70.tar.bz2 px4-firmware-b5e70927d73f6f2f3e17330c9817e6a5d4e1fc70.zip |
Merge remote-tracking branch 'upstream/master' into mtecs_estimator
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 11 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 51 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.h | 1 |
4 files changed, 15 insertions, 54 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 954431fa3..fe4eb66a9 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 @@ -235,8 +235,6 @@ private: float throttle_land_max; - float loiter_hold_radius; - float heightrate_p; float speedrate_p; @@ -280,8 +278,6 @@ private: param_t throttle_land_max; - param_t loiter_hold_radius; - param_t heightrate_p; param_t speedrate_p; @@ -459,7 +455,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); _parameter_handles.l1_damping = param_find("FW_L1_DAMPING"); - _parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R"); _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); @@ -531,7 +526,6 @@ FixedwingPositionControl::parameters_update() /* L1 control parameters */ param_get(_parameter_handles.l1_damping, &(_parameters.l1_damping)); param_get(_parameter_handles.l1_period, &(_parameters.l1_period)); - param_get(_parameter_handles.loiter_hold_radius, &(_parameters.loiter_hold_radius)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index f258f77da..52128e1b7 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -72,17 +72,6 @@ PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f); PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); /** - * Default Loiter Radius - * - * This radius is used when no other loiter radius is set. - * - * @min 10.0 - * @max 100.0 - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f); - -/** * Cruise throttle * * This is the throttle setting required to achieve the desired cruise speed. Most airframes have a value of 0.5-0.7. diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 264cfd875..e300be074 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -790,9 +790,14 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav { switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { - /* Start sending parameters */ - mavlink_pm_start_queued_send(); - mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); + mavlink_param_request_list_t req; + mavlink_msg_param_request_list_decode(msg, &req); + if (req.target_system == mavlink_system.sysid && + (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { + /* Start sending parameters */ + mavlink_pm_start_queued_send(); + mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); + } } break; case MAVLINK_MSG_ID_PARAM_SET: { @@ -954,6 +959,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->current_partner_compid = 0; state->timestamp_lastaction = 0; state->timestamp_last_send_setpoint = 0; + state->timestamp_last_send_request = 0; state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; state->current_dataman_id = 0; } @@ -1070,6 +1076,7 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u wpr.seq = seq; mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr); mavlink_missionlib_send_message(&msg); + _wpm->timestamp_last_send_request = hrt_absolute_time(); if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); } @@ -1112,6 +1119,10 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) _wpm->current_state = MAVLINK_WPM_STATE_IDLE; _wpm->current_partner_sysid = 0; _wpm->current_partner_compid = 0; + + } else if (now - _wpm->timestamp_last_send_request > 500000 && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + /* try to get WP again after short timeout */ + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } @@ -1174,11 +1185,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("IGN WP CURR CMD: Busy"); } } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); } } break; @@ -1211,11 +1217,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("IGN REQUEST LIST: Busy"); } } - - } else { - mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch"); - - if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); } } break; @@ -1304,12 +1305,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); } - - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } @@ -1368,12 +1363,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); } } - - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } break; @@ -1473,11 +1462,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1513,13 +1497,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); } } - - - } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - - mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 25c0da820..40edc4b85 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -93,6 +93,7 @@ struct mavlink_wpm_storage { uint8_t current_partner_compid; uint64_t timestamp_lastaction; uint64_t timestamp_last_send_setpoint; + uint64_t timestamp_last_send_request; uint32_t timeout; int current_dataman_id; }; |