diff options
author | Ivan Ovinnikov <oivan@ethz.ch> | 2012-08-07 14:18:09 +0200 |
---|---|---|
committer | Ivan Ovinnikov <oivan@ethz.ch> | 2012-08-07 14:18:09 +0200 |
commit | 9536bfa3ca239e310be033e8d83a7cc293ebfff6 (patch) | |
tree | c4681da8a3d66f7993ae4cebb887ba3103e53029 /apps/mavlink/mavlink.c | |
parent | 2b09a7914f186831e5a4fca6133355f8aadbe428 (diff) | |
download | px4-firmware-9536bfa3ca239e310be033e8d83a7cc293ebfff6.tar.gz px4-firmware-9536bfa3ca239e310be033e8d83a7cc293ebfff6.tar.bz2 px4-firmware-9536bfa3ca239e310be033e8d83a7cc293ebfff6.zip |
HIL fixed, fixedwing control fixes
Diffstat (limited to 'apps/mavlink/mavlink.c')
-rw-r--r-- | apps/mavlink/mavlink.c | 111 |
1 files changed, 56 insertions, 55 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index d9dd09669..74f7b2099 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -455,7 +455,7 @@ static void *uorb_receiveloop(void *arg) /* struct already globally allocated */ /* subscribe to ORB for fixed wing control */ int fw_sub = orb_subscribe(ORB_ID(fixedwing_control)); - orb_set_interval(fw_sub, 200); /* 5 Hz updates */ + orb_set_interval(fw_sub, 50); /* 20 Hz updates */ fds[fdsc_count].fd = fw_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -615,6 +615,61 @@ static void *uorb_receiveloop(void *arg) mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, fw_control.timestamp / 1000, fw_control.attitude_control_output[0], fw_control.attitude_control_output[1], fw_control.attitude_control_output[2], fw_control.attitude_control_output[3]); + + /* Only send in HIL mode */ + if (v_status.mode & MAV_MODE_FLAG_HIL_ENABLED) { + /* Send the desired attitude from RC or from the autonomous controller */ + // XXX it should not depend on a RC setting, but on a system_state value + + float roll_ail, pitch_elev, throttle, yaw_rudd; + + if (rc.chan[rc.function[OVERRIDE]].scale < 2000) { + + //orb_copy(ORB_ID(fixedwing_control), fixed_wing_control_sub, &fixed_wing_control); + roll_ail = fw_control.attitude_control_output[ROLL]; + pitch_elev = fw_control.attitude_control_output[PITCH]; + throttle = fw_control.attitude_control_output[THROTTLE]; + yaw_rudd = fw_control.attitude_control_output[YAW]; + + } else { + + roll_ail = rc.chan[rc.function[ROLL]].scale; + pitch_elev = rc.chan[rc.function[PITCH]].scale; + throttle = rc.chan[rc.function[THROTTLE]].scale; + yaw_rudd = rc.chan[rc.function[YAW]].scale; + } + + /* hacked HIL implementation in order for the APM Planner to work + * (correct cmd: mavlink_msg_hil_controls_send()) + */ + + mavlink_msg_rc_channels_scaled_send(chan, + hrt_absolute_time(), + 0, // port 0 + roll_ail, + pitch_elev, + throttle, + yaw_rudd, + 0, + 0, + 0, + 0, + 1 /*rssi=1*/); + + /* correct command duplicate */ + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + roll_ail, + pitch_elev, + yaw_rudd, + throttle, + 0, + 0, + 0, + 0, + 32, /* HIL_MODE */ + 0); + } } /* --- VEHICLE GLOBAL POSITION --- */ @@ -1144,60 +1199,6 @@ int mavlink_main(int argc, char *argv[]) lowspeed_counter++; - /* Only send in HIL mode */ - if (v_status.mode & MAV_MODE_FLAG_HIL_ENABLED) { - /* Send the desired attitude from RC or from the autonomous controller */ - // XXX it should not depend on a RC setting, but on a system_state value - - float roll_ail, pitch_elev, throttle, yaw_rudd; - - if (rc.chan[rc.function[OVERRIDE]].scale < 2000) { - - //orb_copy(ORB_ID(fixedwing_control), fixed_wing_control_sub, &fixed_wing_control); - roll_ail = fw_control.attitude_control_output[ROLL]; - pitch_elev = fw_control.attitude_control_output[PITCH]; - throttle = fw_control.attitude_control_output[THROTTLE]; - yaw_rudd = fw_control.attitude_control_output[YAW]; - - } else { - - roll_ail = rc.chan[rc.function[ROLL]].scale; - pitch_elev = rc.chan[rc.function[PITCH]].scale; - throttle = rc.chan[rc.function[THROTTLE]].scale; - yaw_rudd = rc.chan[rc.function[YAW]].scale; - } - - /* hacked HIL implementation in order for the APM Planner to work - * (correct cmd: mavlink_msg_hil_controls_send()) - */ - - mavlink_msg_rc_channels_scaled_send(chan, - hrt_absolute_time(), - 0, // port 0 - roll_ail, - pitch_elev, - throttle, - yaw_rudd, - 0, - 0, - 0, - 0, - 1 /*rssi=1*/); - /* correct command duplicate */ - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - roll_ail, - pitch_elev, - yaw_rudd, - throttle, - 0, - 0, - 0, - 0, - 32, /* HIL_MODE */ - 0); - } - /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); usleep(50000); |