diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2015-03-07 14:07:41 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-03-08 07:49:00 +0100 |
commit | 61437a5587b20e7e3c79fd1ab91e945dc0c316fb (patch) | |
tree | 27483c540d9be8899ce6e40a839b54a1b0514a3c /src | |
parent | 5c3f4d21944fb779feade46e1aba81ca5705462f (diff) | |
download | px4-firmware-61437a5587b20e7e3c79fd1ab91e945dc0c316fb.tar.gz px4-firmware-61437a5587b20e7e3c79fd1ab91e945dc0c316fb.tar.bz2 px4-firmware-61437a5587b20e7e3c79fd1ab91e945dc0c316fb.zip |
MAVLink app: Do no allocate memory statically, but only on execution on stack.
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 51 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 2 |
2 files changed, 27 insertions, 26 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0c34fc58a..d46dba317 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -127,6 +127,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _hil_local_proj_inited(0), _hil_local_alt0(0.0f), _hil_local_proj_ref{}, + _offboard_control_mode{}, + _rates_sp{}, _time_offset_avg_alpha(0.6), _time_offset(0) { @@ -756,8 +758,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) mavlink_set_attitude_target_t set_attitude_target; mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target); - static struct offboard_control_mode_s offboard_control_mode = {}; - /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == set_attitude_target.target_system || set_attitude_target.target_system == 0) && @@ -765,7 +765,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) set_attitude_target.target_component == 0)) { /* set correct ignore flags for thrust field: copy from mavlink message */ - offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); + _offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); /* * The tricky part in parsing this message is that the offboard sender *can* set attitude and thrust @@ -782,26 +782,26 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7); bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); - if (ignore_bodyrate && ignore_attitude && !offboard_control_mode.ignore_thrust) { + if (ignore_bodyrate && ignore_attitude && !_offboard_control_mode.ignore_thrust) { /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ - offboard_control_mode.ignore_bodyrate = ignore_bodyrate && offboard_control_mode.ignore_bodyrate; - offboard_control_mode.ignore_attitude = ignore_attitude && offboard_control_mode.ignore_attitude; + _offboard_control_mode.ignore_bodyrate = ignore_bodyrate && _offboard_control_mode.ignore_bodyrate; + _offboard_control_mode.ignore_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude; } else { - offboard_control_mode.ignore_bodyrate = ignore_bodyrate; - offboard_control_mode.ignore_attitude = ignore_attitude; + _offboard_control_mode.ignore_bodyrate = ignore_bodyrate; + _offboard_control_mode.ignore_attitude = ignore_attitude; } - offboard_control_mode.ignore_position = true; - offboard_control_mode.ignore_velocity = true; - offboard_control_mode.ignore_acceleration_force = true; + _offboard_control_mode.ignore_position = true; + _offboard_control_mode.ignore_velocity = true; + _offboard_control_mode.ignore_acceleration_force = true; - offboard_control_mode.timestamp = hrt_absolute_time(); + _offboard_control_mode.timestamp = hrt_absolute_time(); if (_offboard_control_mode_pub < 0) { - _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); + _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &_offboard_control_mode); } else { - orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); + orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &_offboard_control_mode); } /* If we are in offboard control mode and offboard control loop through is enabled @@ -816,14 +816,14 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if (_control_mode.flag_control_offboard_enabled) { /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ - if (!(offboard_control_mode.ignore_attitude)) { + if (!(_offboard_control_mode.ignore_attitude)) { static struct vehicle_attitude_setpoint_s att_sp = {}; att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body); att_sp.R_valid = true; - if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid att_sp.thrust = set_attitude_target.thrust; } att_sp.yaw_sp_move_rate = 0.0; @@ -838,20 +838,19 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ ///XXX add support for ignoring individual axes - if (!(offboard_control_mode.ignore_bodyrate)) { - static struct vehicle_rates_setpoint_s rates_sp = {}; - rates_sp.timestamp = hrt_absolute_time(); - rates_sp.roll = set_attitude_target.body_roll_rate; - rates_sp.pitch = set_attitude_target.body_pitch_rate; - rates_sp.yaw = set_attitude_target.body_yaw_rate; - if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid - rates_sp.thrust = set_attitude_target.thrust; + if (!(_offboard_control_mode.ignore_bodyrate)) { + _rates_sp.timestamp = hrt_absolute_time(); + _rates_sp.roll = set_attitude_target.body_roll_rate; + _rates_sp.pitch = set_attitude_target.body_pitch_rate; + _rates_sp.yaw = set_attitude_target.body_yaw_rate; + if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + _rates_sp.thrust = set_attitude_target.thrust; } if (_att_sp_pub < 0) { - _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); } else { - orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); + orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp); } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 4886bbd0a..b510dbbb7 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -182,6 +182,8 @@ private: bool _hil_local_proj_inited; float _hil_local_alt0; struct map_projection_reference_s _hil_local_proj_ref; + struct offboard_control_mode_s _offboard_control_mode; + struct vehicle_rates_setpoint_s _rates_sp; double _time_offset_avg_alpha; uint64_t _time_offset; |