dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' into rc_timeout

This commit is contained in:
Anton Babushkin 2014-04-06 22:20:18 +04:00
parent a4ba705e2f ceb287504c
commit b6e00431dc
3 changed files with 42 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -671,18 +671,24 @@ FixedwingAttitudeControl::task_main()
float airspeed; float airspeed;
/* if airspeed is smaller than min, the sensor is not giving good readings */ /* if airspeed is not updating, we assume the normal average speed */
if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || if (!isfinite(_airspeed.true_airspeed_m_s) ||
!isfinite(_airspeed.indicated_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
airspeed = _parameters.airspeed_trim; airspeed = _parameters.airspeed_trim;
} else { } else {
airspeed = _airspeed.indicated_airspeed_m_s; airspeed = _airspeed.true_airspeed_m_s;
} }
float airspeed_scaling = _parameters.airspeed_trim / airspeed; /*
//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); * For scaling our actuators using anything less than the min (close to stall)
* speed doesn't make any sense - its the strongest reasonable deflection we
* want to do in flight and its the baseline a human pilot would choose.
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);
float roll_sp = _parameters.rollsp_offset_rad; float roll_sp = _parameters.rollsp_offset_rad;
float pitch_sp = _parameters.pitchsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad;

60
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -222,12 +222,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
vcmd.source_component = msg->compid; vcmd.source_component = msg->compid;
vcmd.confirmation = cmd_mavlink.confirmation; vcmd.confirmation = cmd_mavlink.confirmation;
/* check if topic is advertised */ if (_cmd_pub < 0) {
if (_cmd_pub <= 0) {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else { } else {
/* publish */
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
} }
} }
@ -253,7 +251,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
f.quality = flow.quality; f.quality = flow.quality;
f.sensor_id = flow.sensor_id; f.sensor_id = flow.sensor_id;
if (_flow_pub <= 0) { if (_flow_pub < 0) {
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f); _flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
} else { } else {
@ -287,7 +285,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
vcmd.source_component = msg->compid; vcmd.source_component = msg->compid;
vcmd.confirmation = 1; vcmd.confirmation = 1;
if (_cmd_pub <= 0) { if (_cmd_pub < 0) {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else { } else {
@ -312,7 +310,7 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
vicon_position.pitch = pos.pitch; vicon_position.pitch = pos.pitch;
vicon_position.yaw = pos.yaw; vicon_position.yaw = pos.yaw;
if (_vicon_position_pub <= 0) { if (_vicon_position_pub < 0) {
_vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); _vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
} else { } else {
@ -373,7 +371,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
offboard_control_sp.timestamp = hrt_absolute_time(); offboard_control_sp.timestamp = hrt_absolute_time();
if (_offboard_control_sp_pub <= 0) { if (_offboard_control_sp_pub < 0) {
_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
} else { } else {
@ -401,7 +399,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
tstatus.rxerrors = rstatus.rxerrors; tstatus.rxerrors = rstatus.rxerrors;
tstatus.fixed = rstatus.fixed; tstatus.fixed = rstatus.fixed;
if (_telemetry_status_pub <= 0) { if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
} else { } else {
@ -428,7 +426,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
rc.chan[2].scaled = man.r / 1000.0f; rc.chan[2].scaled = man.r / 1000.0f;
rc.chan[3].scaled = man.z / 1000.0f; rc.chan[3].scaled = man.z / 1000.0f;
if (_rc_pub == 0) { if (_rc_pub < 0) {
_rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
} else { } else {
@ -450,7 +448,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
manual.yaw = man.r / 1000.0f; manual.yaw = man.r / 1000.0f;
manual.throttle = man.z / 1000.0f; manual.throttle = man.z / 1000.0f;
if (_manual_pub == 0) { if (_manual_pub < 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
} else { } else {
@ -619,11 +617,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_sensors.differential_pressure_timestamp = timestamp; hil_sensors.differential_pressure_timestamp = timestamp;
/* publish combined sensor topic */ /* publish combined sensor topic */
if (_sensors_pub > 0) { if (_sensors_pub < 0) {
orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
} else { } else {
_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
} }
} }
@ -638,11 +636,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_battery_status.current_a = 10.0f; hil_battery_status.current_a = 10.0f;
hil_battery_status.discharged_mah = -1.0f; hil_battery_status.discharged_mah = -1.0f;
if (_battery_pub > 0) { if (_battery_pub < 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
} else { } else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
} }
} }
@ -694,11 +692,11 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
hil_gps.fix_type = gps.fix_type; hil_gps.fix_type = gps.fix_type;
hil_gps.satellites_visible = gps.satellites_visible; hil_gps.satellites_visible = gps.satellites_visible;
if (_gps_pub > 0) { if (_gps_pub < 0) {
orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
} else { } else {
_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
} }
} }
@ -752,11 +750,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_attitude.pitchspeed = hil_state.pitchspeed; hil_attitude.pitchspeed = hil_state.pitchspeed;
hil_attitude.yawspeed = hil_state.yawspeed; hil_attitude.yawspeed = hil_state.yawspeed;
if (_attitude_pub > 0) { if (_attitude_pub < 0) {
orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
} else { } else {
_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
} }
} }
@ -775,11 +773,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_global_pos.vel_d = hil_state.vz / 100.0f; hil_global_pos.vel_d = hil_state.vz / 100.0f;
hil_global_pos.yaw = hil_attitude.yaw; hil_global_pos.yaw = hil_attitude.yaw;
if (_global_pos_pub > 0) { if (_global_pos_pub < 0) {
orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
} else { } else {
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
} }
} }
@ -816,11 +814,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
hil_local_pos.landed = landed; hil_local_pos.landed = landed;
if (_local_pos_pub > 0) { if (_local_pos_pub < 0) {
orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
} else { } else {
_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
} }
} }
@ -857,11 +855,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_battery_status.current_a = 10.0f; hil_battery_status.current_a = 10.0f;
hil_battery_status.discharged_mah = -1.0f; hil_battery_status.discharged_mah = -1.0f;
if (_battery_pub > 0) { if (_battery_pub < 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
} else { } else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
} }
} }
} }

2
src/modules/mavlink/mavlink_receiver.h
View File

@ -120,7 +120,6 @@ private:
mavlink_status_t status; mavlink_status_t status;
struct vehicle_local_position_s hil_local_pos; struct vehicle_local_position_s hil_local_pos;
int _manual_sub;
orb_advert_t _global_pos_pub; orb_advert_t _global_pos_pub;
orb_advert_t _local_pos_pub; orb_advert_t _local_pos_pub;
orb_advert_t _attitude_pub; orb_advert_t _attitude_pub;
@ -139,6 +138,7 @@ private:
orb_advert_t _telemetry_status_pub; orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub; orb_advert_t _rc_pub;
orb_advert_t _manual_pub; orb_advert_t _manual_pub;
int _manual_sub;
int _hil_frames; int _hil_frames;
uint64_t _old_timestamp; uint64_t _old_timestamp;
bool _hil_local_proj_inited; bool _hil_local_proj_inited;