diff options
author | Julian Oes <julian@oes.ch> | 2014-06-27 10:43:29 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-06-27 12:54:52 +0200 |
commit | 4c4d14347efe9d12712399a1a5eb0140fdd113fd (patch) | |
tree | 81fa4685dbdf8f250dc6afbe98e7901f165d9733 | |
parent | a122e88893f4b983a6e8e2e2838f9a16f055c8d1 (diff) | |
download | px4-firmware-4c4d14347efe9d12712399a1a5eb0140fdd113fd.tar.gz px4-firmware-4c4d14347efe9d12712399a1a5eb0140fdd113fd.tar.bz2 px4-firmware-4c4d14347efe9d12712399a1a5eb0140fdd113fd.zip |
mavlink: only save a offboard_control, don't publish to setpoint topics (let the navigator do this)
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 154 |
1 files changed, 11 insertions, 143 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index aaae0933e..c4955f1b3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -355,70 +355,22 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) void MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg) { - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); + mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control; + mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control); - if (mavlink_system.sysid < 4) { + /* Only accept system IDs from 1 to 4 + * TODO: check for own system ID */ + if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) { struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - enum OFFBOARD_CONTROL_MODE ml_mode = OFFBOARD_CONTROL_MODE_DIRECT; - bool ml_armed = false; + /* Convert values * 1000 back */ + offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f; + offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f; + offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f; + offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f; - switch (quad_motors_setpoint.mode) { - case 0: - break; - - case 1: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; - ml_armed = true; - - break; - - case 2: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; - ml_armed = true; - - break; - - case 3: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; - ml_armed = true; - - break; - - case 4: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; - ml_armed = true; - - break; - default: - break; - } - - if (ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES || ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { - - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - - } else if (ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { - - /*Temporary hack to use set_quad_swarm_roll_pitch_yaw_thrust msg for position or velocity */ - /* Converts INT16 centimeters to float meters */ - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / 1000.0f; - } - - if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { - ml_armed = false; - } - - offboard_control_sp.armed = ml_armed; - offboard_control_sp.mode = ml_mode; + offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode; offboard_control_sp.timestamp = hrt_absolute_time(); @@ -428,90 +380,6 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message } else { orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); } - - bool updated; - orb_check(_control_mode_sub, &updated); - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); - } - - if (_control_mode.flag_control_offboard_enabled) { - if (_control_mode.flag_control_position_enabled) { - // TODO Use something else then quad_swarm_roll_pitch_yaw_thrust - struct vehicle_local_position_setpoint_s loc_pos_sp; - memset(&loc_pos_sp, 0, sizeof(loc_pos_sp)); - - loc_pos_sp.x = offboard_control_sp.p1; - loc_pos_sp.y = offboard_control_sp.p2; - loc_pos_sp.yaw = offboard_control_sp.p3; - loc_pos_sp.z = -offboard_control_sp.p4; - - if (_local_pos_sp_pub < 0) { - _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &loc_pos_sp); - - } else { - orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &loc_pos_sp); - } - - } else if (_control_mode.flag_control_velocity_enabled) { - /* velocity control */ - struct vehicle_global_velocity_setpoint_s global_vel_sp; - memset(&global_vel_sp, 0, sizeof(&global_vel_sp)); - - global_vel_sp.vx = offboard_control_sp.p1; - global_vel_sp.vy = offboard_control_sp.p2; - global_vel_sp.vz = offboard_control_sp.p3; - - if (_global_vel_sp_pub < 0) { - _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); - - } else { - orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &global_vel_sp); - } - - } else if (_control_mode.flag_control_attitude_enabled) { - /* attitude control */ - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - - att_sp.roll_body = offboard_control_sp.p1; - att_sp.pitch_body = offboard_control_sp.p2; - att_sp.yaw_body = offboard_control_sp.p3; - att_sp.thrust = offboard_control_sp.p4; - - att_sp.timestamp = hrt_absolute_time(); - - if (_att_sp_pub < 0) { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - - } else { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); - } - - } else if (_control_mode.flag_control_rates_enabled) { - /* rates control */ - struct vehicle_rates_setpoint_s rates_sp; - memset(&rates_sp, 0, sizeof(rates_sp)); - - rates_sp.roll = offboard_control_sp.p1; - rates_sp.pitch = offboard_control_sp.p2; - rates_sp.yaw = offboard_control_sp.p3; - rates_sp.thrust = offboard_control_sp.p4; - - rates_sp.timestamp = hrt_absolute_time(); - - if (_rates_sp_pub < 0) { - _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); - } - - } else { - /* direct control */ - // TODO - } - } } } |