From e291af990fd9a4f447cbad2416b78d031cd33f5c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 00:24:14 +0400 Subject: mavlink: adding message stream by name implemnted, mavlink streams definitions and formatters moved to mavlink_messages.h/cpp, mavlink_orb_listener class and thread removed --- src/modules/mavlink/mavlink_messages.cpp | 613 +++++++++++++++++++++++++++++++ 1 file changed, 613 insertions(+) create mode 100644 src/modules/mavlink/mavlink_messages.cpp (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp new file mode 100644 index 000000000..77ec344dc --- /dev/null +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -0,0 +1,613 @@ +/* + * mavlink_messages.cpp + * + * Created on: 25.02.2014 + * Author: ton + */ + +#include + +#include +#include + +#include "mavlink_messages.h" + + +struct msgs_list_s msgs_list[] = { + { + .name = "HEARTBEAT", + .callback = msg_heartbeat, + .topics = { ORB_ID(vehicle_status), ORB_ID(position_setpoint_triplet), nullptr }, + .sizes = { sizeof(struct vehicle_status_s), sizeof(struct position_setpoint_triplet_s) } + }, + { + .name = "SYS_STATUS", + .callback = msg_sys_status, + .topics = { ORB_ID(vehicle_status), nullptr }, + .sizes = { sizeof(struct vehicle_status_s) } + }, + { .name = nullptr } +}; + +void +msg_heartbeat(const MavlinkStream *stream) +{ + struct vehicle_status_s *status = (struct vehicle_status_s *)stream->subscriptions[0]->data; + struct position_setpoint_triplet_s *pos_sp_triplet = (struct position_setpoint_triplet_s *)stream->subscriptions[1]->data; + + uint8_t mavlink_state = 0; + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + + /* HIL */ + if (status->hil_state == HIL_STATE_ON) { + mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + /* arming state */ + if (status->arming_state == ARMING_STATE_ARMED + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } + + /* main state */ + mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + union px4_custom_mode custom_mode; + custom_mode.data = 0; + if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { + /* use main state when navigator is not active */ + if (status->main_state == MAIN_STATE_MANUAL) { + mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (status->main_state == MAIN_STATE_SEATBELT) { + mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + } else if (status->main_state == MAIN_STATE_EASY) { + mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + } else if (status->main_state == MAIN_STATE_AUTO) { + mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } + } else { + /* use navigation state when navigator is active */ + mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + if (pos_sp_triplet->nav_state == NAV_STATE_READY) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + } + } + mavlink_custom_mode = custom_mode.data; + + /* set system state */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + mavlink_state = MAV_STATE_UNINIT; + } else if (status->arming_state == ARMING_STATE_ARMED) { + mavlink_state = MAV_STATE_ACTIVE; + } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + mavlink_state = MAV_STATE_CRITICAL; + } else if (status->arming_state == ARMING_STATE_STANDBY) { + mavlink_state = MAV_STATE_STANDBY; + } else if (status->arming_state == ARMING_STATE_REBOOT) { + mavlink_state = MAV_STATE_POWEROFF; + } else { + mavlink_state = MAV_STATE_CRITICAL; + } + + /* send heartbeat */ + mavlink_msg_heartbeat_send(stream->mavlink->get_chan(), + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_base_mode, + mavlink_custom_mode, + mavlink_state); +} + +void +msg_sys_status(const MavlinkStream *stream) +{ + struct vehicle_status_s *status = (struct vehicle_status_s *)stream->subscriptions[0]->data; + + mavlink_msg_sys_status_send(stream->mavlink->get_chan(), + status->onboard_control_sensors_present, + status->onboard_control_sensors_enabled, + status->onboard_control_sensors_health, + status->load * 1000.0f, + status->battery_voltage * 1000.0f, + status->battery_current * 1000.0f, + status->battery_remaining, + status->drop_rate_comm, + status->errors_comm, + status->errors_count1, + status->errors_count2, + status->errors_count3, + status->errors_count4); +} + +//void +//MavlinkOrbListener::l_sensor_combined(const struct listener *l) +//{ +// struct sensor_combined_s raw; +// +// /* copy sensors raw data into local buffer */ +// orb_copy(ORB_ID(sensor_combined), l->mavlink->get_subs()->sensor_sub, &raw); +// +// /* mark individual fields as _mavlink->get_chan()ged */ +// uint16_t fields_updated = 0; +// +// // if (accel_counter != raw.accelerometer_counter) { +// // /* mark first three dimensions as _mavlink->get_chan()ged */ +// // fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); +// // accel_counter = raw.accelerometer_counter; +// // } +// +// // if (gyro_counter != raw.gyro_counter) { +// // /* mark second group dimensions as _mavlink->get_chan()ged */ +// // fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); +// // gyro_counter = raw.gyro_counter; +// // } +// +// // if (mag_counter != raw.magnetometer_counter) { +// // /* mark third group dimensions as _mavlink->get_chan()ged */ +// // fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); +// // mag_counter = raw.magnetometer_counter; +// // } +// +// // if (baro_counter != raw.baro_counter) { +// // /* mark last group dimensions as _mavlink->get_chan()ged */ +// // fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); +// // baro_counter = raw.baro_counter; +// // } +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_highres_imu_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp, +// raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], +// raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], +// raw.gyro_rad_s[1], raw.gyro_rad_s[2], +// raw.magnetometer_ga[0], +// raw.magnetometer_ga[1], raw.magnetometer_ga[2], +// raw.baro_pres_mbar, raw.differential_pressure_pa, +// raw.baro_alt_meter, raw.baro_temp_celcius, +// fields_updated); +// +// l->listener->sensors_raw_counter++; +//} +// +//void +//MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) +//{ +// /* copy attitude data into local buffer */ +// orb_copy(ORB_ID(vehicle_attitude), l->mavlink->get_subs()->att_sub, &l->listener->att); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { +// /* send sensor values */ +// mavlink_msg_attitude_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// l->listener->att.roll, +// l->listener->att.pitch, +// l->listener->att.yaw, +// l->listener->att.rollspeed, +// l->listener->att.pitchspeed, +// l->listener->att.yawspeed); +// +// /* limit VFR message rate to 10Hz */ +// hrt_abstime t = hrt_absolute_time(); +// if (t >= l->listener->last_sent_vfr + 100000) { +// l->listener->last_sent_vfr = t; +// float groundspeed = sqrtf(l->listener->global_pos.vel_n * l->listener->global_pos.vel_n + l->listener->global_pos.vel_e * l->listener->global_pos.vel_e); +// uint16_t heading = _wrap_2pi(l->listener->att.yaw) * M_RAD_TO_DEG_F; +// float throttle = l->listener->armed.armed ? l->listener->actuators_0.control[3] * 100.0f : 0.0f; +// mavlink_msg_vfr_hud_send(l->mavlink->get_chan(), l->listener->airspeed.true_airspeed_m_s, groundspeed, heading, throttle, l->listener->global_pos.alt, -l->listener->global_pos.vel_d); +// } +// +// /* send quaternion values if it exists */ +// if(l->listener->att.q_valid) { +// mavlink_msg_attitude_quaternion_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// l->listener->att.q[0], +// l->listener->att.q[1], +// l->listener->att.q[2], +// l->listener->att.q[3], +// l->listener->att.rollspeed, +// l->listener->att.pitchspeed, +// l->listener->att.yawspeed); +// } +// } +// +// l->listener->attitude_counter++; +//} +// +//void +//MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l) +//{ +// struct vehicle_gps_position_s gps; +// +// /* copy gps data into local buffer */ +// orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_subs()->gps_sub, &gps); +// +// /* GPS COG is 0..2PI in degrees * 1e2 */ +// float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F; +// +// /* GPS position */ +// mavlink_msg_gps_raw_int_send(l->mavlink->get_chan(), +// gps.timestamp_position, +// gps.fix_type, +// gps.lat, +// gps.lon, +// gps.alt, +// cm_uint16_from_m_float(gps.eph_m), +// cm_uint16_from_m_float(gps.epv_m), +// gps.vel_m_s * 1e2f, // from m/s to cm/s +// cog_deg * 1e2f, // from deg to deg * 100 +// gps.satellites_visible); +// +// /* update SAT info every 10 seconds */ +// if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) { +// mavlink_msg_gps_status_send(l->mavlink->get_chan(), +// gps.satellites_visible, +// gps.satellite_prn, +// gps.satellite_used, +// gps.satellite_elevation, +// gps.satellite_azimuth, +// gps.satellite_snr); +// } +// +// l->listener->gps_counter++; +//} +// +// +//void +//MavlinkOrbListener::l_rc_channels(const struct listener *l) +//{ +// /* copy rc channels into local buffer */ +// orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc); +// // XXX Add RC channels scaled message here +//} +// +//void +//MavlinkOrbListener::l_input_rc(const struct listener *l) +//{ +// /* copy rc _mavlink->get_chan()nels into local buffer */ +// orb_copy(ORB_ID(input_rc), l->mavlink->get_subs()->input_rc_sub, &l->listener->rc_raw); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { +// +// const unsigned port_width = 8; +// +// for (unsigned i = 0; (i * port_width) < l->listener->rc_raw.channel_count; i++) { +// /* Channels are sent in MAVLink main loop at a fixed interval */ +// mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(), +// l->listener->rc_raw.timestamp_publication / 1000, +// i, +// (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 2) ? l->listener->rc_raw.values[(i * port_width) + 2] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 3) ? l->listener->rc_raw.values[(i * port_width) + 3] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 4) ? l->listener->rc_raw.values[(i * port_width) + 4] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 5) ? l->listener->rc_raw.values[(i * port_width) + 5] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 6) ? l->listener->rc_raw.values[(i * port_width) + 6] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 7) ? l->listener->rc_raw.values[(i * port_width) + 7] : UINT16_MAX, +// l->listener->rc_raw.rssi); +// } +// } +//} +// +//void +//MavlinkOrbListener::l_global_position(const struct listener *l) +//{ +// /* copy global position data into local buffer */ +// orb_copy(ORB_ID(vehicle_global_position), l->mavlink->get_subs()->global_pos_sub, &l->listener->global_pos); +// +// mavlink_msg_global_position_int_send(l->mavlink->get_chan(), +// l->listener->global_pos.timestamp / 1000, +// l->listener->global_pos.lat * 1e7, +// l->listener->global_pos.lon * 1e7, +// l->listener->global_pos.alt * 1000.0f, +// (l->listener->global_pos.alt - l->listener->home.alt) * 1000.0f, +// l->listener->global_pos.vel_n * 100.0f, +// l->listener->global_pos.vel_e * 100.0f, +// l->listener->global_pos.vel_d * 100.0f, +// _wrap_2pi(l->listener->global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); +//} +// +//void +//MavlinkOrbListener::l_local_position(const struct listener *l) +//{ +// /* copy local position data into local buffer */ +// orb_copy(ORB_ID(vehicle_local_position), l->mavlink->get_subs()->local_pos_sub, &l->listener->local_pos); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_local_position_ned_send(l->mavlink->get_chan(), +// l->listener->local_pos.timestamp / 1000, +// l->listener->local_pos.x, +// l->listener->local_pos.y, +// l->listener->local_pos.z, +// l->listener->local_pos.vx, +// l->listener->local_pos.vy, +// l->listener->local_pos.vz); +//} +// +//void +//MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) +//{ +// struct position_setpoint_triplet_s triplet; +// orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet); +// +// if (!triplet.current.valid) +// return; +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_global_position_setpoint_int_send(l->mavlink->get_chan(), +// MAV_FRAME_GLOBAL, +// (int32_t)(triplet.current.lat * 1e7d), +// (int32_t)(triplet.current.lon * 1e7d), +// (int32_t)(triplet.current.alt * 1e3f), +// (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); +//} +// +//void +//MavlinkOrbListener::l_local_position_setpoint(const struct listener *l) +//{ +// struct vehicle_local_position_setpoint_s local_sp; +// +// /* copy local position data into local buffer */ +// orb_copy(ORB_ID(vehicle_local_position_setpoint), l->mavlink->get_subs()->spl_sub, &local_sp); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_local_position_setpoint_send(l->mavlink->get_chan(), +// MAV_FRAME_LOCAL_NED, +// local_sp.x, +// local_sp.y, +// local_sp.z, +// local_sp.yaw); +//} +// +//void +//MavlinkOrbListener::l_attitude_setpoint(const struct listener *l) +//{ +// struct vehicle_attitude_setpoint_s att_sp; +// +// /* copy local position data into local buffer */ +// orb_copy(ORB_ID(vehicle_attitude_setpoint), l->mavlink->get_subs()->spa_sub, &att_sp); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(), +// att_sp.timestamp / 1000, +// att_sp.roll_body, +// att_sp.pitch_body, +// att_sp.yaw_body, +// att_sp.thrust); +//} +// +//void +//MavlinkOrbListener::l_vehicle_rates_setpoint(const struct listener *l) +//{ +// struct vehicle_rates_setpoint_s rates_sp; +// +// /* copy local position data into local buffer */ +// orb_copy(ORB_ID(vehicle_rates_setpoint), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(l->mavlink->get_chan(), +// rates_sp.timestamp / 1000, +// rates_sp.roll, +// rates_sp.pitch, +// rates_sp.yaw, +// rates_sp.thrust); +//} +// +//void +//MavlinkOrbListener::l_actuator_outputs(const struct listener *l) +//{ +// struct actuator_outputs_s act_outputs; +// +// orb_id_t ids[] = { +// ORB_ID(actuator_outputs_0), +// ORB_ID(actuator_outputs_1), +// ORB_ID(actuator_outputs_2), +// ORB_ID(actuator_outputs_3) +// }; +// +// /* copy actuator data into local buffer */ +// orb_copy(ids[l->arg], *l->subp, &act_outputs); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { +// mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp / 1000, +// l->arg /* port number - needs GCS support */, +// /* QGC has port number support already */ +// act_outputs.output[0], +// act_outputs.output[1], +// act_outputs.output[2], +// act_outputs.output[3], +// act_outputs.output[4], +// act_outputs.output[5], +// act_outputs.output[6], +// act_outputs.output[7]); +// +// /* only send in HIL mode and only send first group for HIL */ +// if (l->mavlink->get_hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { +// +// /* translate the current syste state to mavlink state and mode */ +// uint8_t mavlink_state = 0; +// uint8_t mavlink_base_mode = 0; +// uint32_t mavlink_custom_mode = 0; +// l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); +// +// /* HIL message as per MAVLink spec */ +// +// /* scale / assign outputs depending on system type */ +// +// if (mavlink_system.type == MAV_TYPE_QUADROTOR) { +// mavlink_msg_hil_controls_send(l->mavlink->get_chan(), +// hrt_absolute_time(), +// ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, +// -1, +// -1, +// -1, +// -1, +// mavlink_base_mode, +// 0); +// +// } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { +// mavlink_msg_hil_controls_send(l->mavlink->get_chan(), +// hrt_absolute_time(), +// ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, +// -1, +// -1, +// mavlink_base_mode, +// 0); +// +// } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { +// mavlink_msg_hil_controls_send(l->mavlink->get_chan(), +// hrt_absolute_time(), +// ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, +// mavlink_base_mode, +// 0); +// +// } else { +// mavlink_msg_hil_controls_send(l->mavlink->get_chan(), +// hrt_absolute_time(), +// (act_outputs.output[0] - 1500.0f) / 500.0f, +// (act_outputs.output[1] - 1500.0f) / 500.0f, +// (act_outputs.output[2] - 1500.0f) / 500.0f, +// (act_outputs.output[3] - 1000.0f) / 1000.0f, +// (act_outputs.output[4] - 1500.0f) / 500.0f, +// (act_outputs.output[5] - 1500.0f) / 500.0f, +// (act_outputs.output[6] - 1500.0f) / 500.0f, +// (act_outputs.output[7] - 1500.0f) / 500.0f, +// mavlink_base_mode, +// 0); +// } +// } +// } +//} +// +//void +//MavlinkOrbListener::l_actuator_armed(const struct listener *l) +//{ +// orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); +//} +// +//void +//MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l) +//{ +// struct manual_control_setpoint_s man_control; +// +// /* copy manual control data into local buffer */ +// orb_copy(ORB_ID(manual_control_setpoint), l->mavlink->get_subs()->man_control_sp_sub, &man_control); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_manual_control_send(l->mavlink->get_chan(), +// mavlink_system.sysid, +// man_control.roll * 1000, +// man_control.pitch * 1000, +// man_control.yaw * 1000, +// man_control.throttle * 1000, +// 0); +//} +// +//void +//MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l) +//{ +// orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { +// /* send, add spaces so that string buffer is at least 10 chars long */ +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// "ctrl0 ", +// l->listener->actuators_0.control[0]); +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// "ctrl1 ", +// l->listener->actuators_0.control[1]); +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// "ctrl2 ", +// l->listener->actuators_0.control[2]); +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// "ctrl3 ", +// l->listener->actuators_0.control[3]); +// } +//} +// +//void +//MavlinkOrbListener::l_debug_key_value(const struct listener *l) +//{ +// struct debug_key_value_s debug; +// +// orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug); +// +// /* Enforce null termination */ +// debug.key[sizeof(debug.key) - 1] = '\0'; +// +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// debug.key, +// debug.value); +//} +// +//void +//MavlinkOrbListener::l_optical_flow(const struct listener *l) +//{ +// struct optical_flow_s flow; +// +// orb_copy(ORB_ID(optical_flow), l->mavlink->get_subs()->optical_flow, &flow); +// +// mavlink_msg_optical_flow_send(l->mavlink->get_chan(), flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, +// flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m); +//} +// +//void +//MavlinkOrbListener::l_home(const struct listener *l) +//{ +// orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home); +// +// mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f); +//} +// +//void +//MavlinkOrbListener::l_airspeed(const struct listener *l) +//{ +// orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed); +//} +// +//void +//MavlinkOrbListener::l_nav_cap(const struct listener *l) +//{ +// +// orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap); +// +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// hrt_absolute_time() / 1000, +// "turn dist", +// l->listener->nav_cap.turn_distance); +// +//} -- cgit v1.2.3 From 769a2af1f8925a2d47fd47a2d25f8d7baac150ec Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 00:38:21 +0400 Subject: mavlink: HIGHRES_IMU message added, default message streams added --- src/modules/mavlink/mavlink_main.cpp | 72 +++++++------------------------- src/modules/mavlink/mavlink_messages.cpp | 17 ++++++++ src/modules/mavlink/mavlink_messages.h | 2 +- 3 files changed, 33 insertions(+), 58 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4a75b00ab..1563a257b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1526,62 +1526,6 @@ Mavlink::task_main(int argc, char *argv[]) /* initialize waypoint manager */ mavlink_wpm_init(wpm); - /* all subscriptions are now active, set up initial guess about rate limits */ -// if (_baudrate >= 230400) { -// /* 200 Hz / 5 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 20); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 20); -// /* 50 Hz / 20 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 30); -// /* 20 Hz / 50 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); -// /* 10 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 100); -// /* 10 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 100); -// -// } else if (_baudrate >= 115200) { -// /* 20 Hz / 50 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 50); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 50); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); -// /* 5 Hz / 200 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); -// /* 5 Hz / 200 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 200); -// /* 2 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); -// -// } else if (_baudrate >= 57600) { -// /* 10 Hz / 100 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 300); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 300); -// /* 10 Hz / 100 ms ATTITUDE */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 200); -// /* 5 Hz / 200 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); -// /* 5 Hz / 200 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); -// /* 2 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); -// /* 2 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 500); -// -// } else { -// /* very low baud rate, limit to 1 Hz / 1000 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 1000); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 1000); -// /* 1 Hz / 1000 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); -// /* 0.5 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); -// /* 0.1 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); -// } - int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); struct mission_result_s mission_result; memset(&mission_result, 0, sizeof(mission_result)); @@ -1595,8 +1539,22 @@ Mavlink::task_main(int argc, char *argv[]) struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->data; + + /* add default streams, intervals depend on baud rate */ +// if (_baudrate >= 230400) { +// } else if (_baudrate >= 115200) { +// } else if (_baudrate >= 57600) { +// } + add_stream("HEARTBEAT", 1000); - add_stream("SYS_STATUS", 100); + add_stream("SYS_STATUS", 1000); + add_stream("HIGHRES_IMU", 300); + add_stream("RAW_IMU", 300); + add_stream("ATTITUDE", 200); + add_stream("NAMED_VALUE_FLOAT", 200); + add_stream("SERVO_OUTPUT_RAW", 500); + add_stream("GPS_RAW_INT", 500); + add_stream("MANUAL_CONTROL", 500); while (!_task_should_exit) { /* main loop */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 77ec344dc..7a56c36a5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -135,6 +135,23 @@ msg_sys_status(const MavlinkStream *stream) status->errors_count4); } +void +msg_highres_imu(const MavlinkStream *stream) +{ + struct sensor_combined_s *sensor = (struct sensor_combined_s *)stream->subscriptions[0]->data; + + uint16_t fields_updated = 0; + + if (stream->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_highres_imu_send(stream->mavlink->get_chan(), sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); +} + //void //MavlinkOrbListener::l_sensor_combined(const struct listener *l) //{ diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index 78279c08f..a6326bad1 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -23,6 +23,6 @@ extern struct msgs_list_s msgs_list[]; static void msg_heartbeat(const MavlinkStream *stream); static void msg_sys_status(const MavlinkStream *stream); - +static void msg_highres_imu(const MavlinkStream *stream); #endif /* MAVLINK_MESSAGES_H_ */ -- cgit v1.2.3 From 7310fd608500be69153c5d033f74b056f1bb986e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 21:28:35 +0400 Subject: mavlink: use inherited classes instead of callbacks for mavlink messages formatting, fixes and cleanup --- src/modules/mavlink/mavlink_main.cpp | 202 +++++------ src/modules/mavlink/mavlink_main.h | 16 +- src/modules/mavlink/mavlink_messages.cpp | 442 ++++++++++++++--------- src/modules/mavlink/mavlink_messages.h | 15 +- src/modules/mavlink/mavlink_orb_subscription.cpp | 36 +- src/modules/mavlink/mavlink_orb_subscription.h | 17 +- src/modules/mavlink/mavlink_receiver.cpp | 2 +- src/modules/mavlink/mavlink_stream.cpp | 49 +-- src/modules/mavlink/mavlink_stream.h | 25 +- 9 files changed, 443 insertions(+), 361 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1563a257b..33d81729c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -152,12 +152,6 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length static void usage(void); -namespace mavlink -{ - - Mavlink *g_mavlink; -} - Mavlink::Mavlink() : device_name("/dev/ttyS1"), _task_should_exit(false), @@ -166,14 +160,13 @@ Mavlink::Mavlink() : thread_running(false), _mavlink_task(-1), _mavlink_incoming_fd(-1), - -/* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")), _mavlink_hil_enabled(false), _subscriptions(nullptr), _streams(nullptr), + mission_pub(-1), - mission_pub(-1) +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { wpm = &wpm_s; fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl; @@ -204,12 +197,14 @@ Mavlink::~Mavlink() } } -void Mavlink::set_mode(enum MAVLINK_MODE mode) +void +Mavlink::set_mode(enum MAVLINK_MODE mode) { _mode = mode; } -int Mavlink::instance_count() +int +Mavlink::instance_count() { /* note: a local buffer count will help if this ever is called often */ Mavlink* inst = ::_head; @@ -222,10 +217,11 @@ int Mavlink::instance_count() return inst_index; } -Mavlink* Mavlink::new_instance() +Mavlink * +Mavlink::new_instance() { - Mavlink* inst = new Mavlink(); - Mavlink* next = ::_head; + Mavlink *inst = new Mavlink(); + Mavlink *next = ::_head; /* create the first instance at _head */ if (::_head == nullptr) { @@ -241,9 +237,10 @@ Mavlink* Mavlink::new_instance() return inst; } -Mavlink* Mavlink::get_instance(unsigned instance) +Mavlink * +Mavlink::get_instance(unsigned instance) { - Mavlink* inst = ::_head; + Mavlink *inst = ::_head; unsigned inst_index = 0; while (inst->_next != nullptr && inst_index < instance) { inst = inst->_next; @@ -257,7 +254,8 @@ Mavlink* Mavlink::get_instance(unsigned instance) return inst; } -int Mavlink::destroy_all_instances() +int +Mavlink::destroy_all_instances() { /* start deleting from the end */ Mavlink *inst_to_del = nullptr; @@ -294,7 +292,8 @@ int Mavlink::destroy_all_instances() return OK; } -bool Mavlink::instance_exists(const char *device_name, Mavlink *self) +bool +Mavlink::instance_exists(const char *device_name, Mavlink *self) { Mavlink* inst = ::_head; while (inst != nullptr) { @@ -307,7 +306,8 @@ bool Mavlink::instance_exists(const char *device_name, Mavlink *self) return false; } -int Mavlink::get_uart_fd(unsigned index) +int +Mavlink::get_uart_fd(unsigned index) { Mavlink* inst = get_instance(index); if (inst) @@ -316,14 +316,16 @@ int Mavlink::get_uart_fd(unsigned index) return -1; } -int Mavlink::get_uart_fd() +int +Mavlink::get_uart_fd() { return _uart; } -int Mavlink::get_channel() +mavlink_channel_t +Mavlink::get_channel() { - return (int)_chan; + return _chan; } /**************************************************************************** @@ -1364,7 +1366,7 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata MavlinkOrbSubscription *sub; LL_FOREACH(_subscriptions, sub) { - if (sub->topic == topic) { + if (sub->get_topic() == topic) { /* already subscribed */ return sub; } @@ -1379,26 +1381,19 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata } int -Mavlink::add_stream(const char *stream_name, const unsigned int interval) +Mavlink::add_stream(const char *stream_name, const float rate) { - uintptr_t arg = 0; - - unsigned int i = 0; - /* search for message with specified name */ - while (msgs_list[i].name != nullptr) { - if (strcmp(stream_name, msgs_list[i].name) == 0) { - /* count topics, array is nullptr-terminated */ - unsigned int topics_n; - for (topics_n = 0; topics_n < MAX_TOPICS_PER_MAVLINK_STREAM; topics_n++) { - if (msgs_list[i].topics[topics_n] == nullptr) { - break; - } - } - MavlinkStream *stream = new MavlinkStream(this, msgs_list[i].callback, topics_n, msgs_list[i].topics, msgs_list[i].sizes, arg, interval); + /* search for stream with specified name */ + for (unsigned int i = 0; streams_list[i] != nullptr; i++) { + if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { + /* create stream copy for each mavlink instance */ + MavlinkStream *stream = streams_list[i]->new_instance(); + stream->set_channel(get_channel()); + stream->set_interval(1000.0f / rate); + stream->subscribe(this); LL_APPEND(_streams, stream); return OK; } - i++; } return ERROR; } @@ -1407,7 +1402,7 @@ int Mavlink::task_main(int argc, char *argv[]) { /* inform about start */ - warnx("Initializing.."); + warnx("start"); fflush(stdout); /* initialize mavlink text message buffering */ @@ -1463,9 +1458,6 @@ Mavlink::task_main(int argc, char *argv[]) bool usb_uart; - /* print welcome text */ - warnx("MAVLink v1.0 serial interface starting..."); - /* inform about mode */ switch (_mode) { case MODE_TX_HEARTBEAT_ONLY: @@ -1510,13 +1502,13 @@ Mavlink::task_main(int argc, char *argv[]) err(1, "could not open %s", device_name); /* create the device node that's used for sending text log messages, etc. */ - register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); + if (instance_count() == 1) { + register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); + } /* initialize logging device */ _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(_mavlink_fd, "[mavlink] started"); - /* Initialize system properties */ mavlink_update_system(); @@ -1537,8 +1529,10 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s)); MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s)); - struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->data; + struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data(); + warnx("started"); + mavlink_log_info(_mavlink_fd, "[mavlink] started"); /* add default streams, intervals depend on baud rate */ // if (_baudrate >= 230400) { @@ -1546,15 +1540,15 @@ Mavlink::task_main(int argc, char *argv[]) // } else if (_baudrate >= 57600) { // } - add_stream("HEARTBEAT", 1000); - add_stream("SYS_STATUS", 1000); - add_stream("HIGHRES_IMU", 300); - add_stream("RAW_IMU", 300); - add_stream("ATTITUDE", 200); - add_stream("NAMED_VALUE_FLOAT", 200); - add_stream("SERVO_OUTPUT_RAW", 500); - add_stream("GPS_RAW_INT", 500); - add_stream("MANUAL_CONTROL", 500); + add_stream("HEARTBEAT", 1.0f); + add_stream("SYS_STATUS", 1.0f); + add_stream("HIGHRES_IMU", 20.0f); +// add_stream("RAW_IMU", 10.0f); + add_stream("ATTITUDE", 20.0f); +// add_stream("NAMED_VALUE_FLOAT", 5.0f); +// add_stream("SERVO_OUTPUT_RAW", 2.0f); +// add_stream("GPS_RAW_INT", 2.0f); +// add_stream("MANUAL_CONTROL", 2.0f); while (!_task_should_exit) { /* main loop */ @@ -1604,31 +1598,24 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); } - mavlink_waypoint_eventloop(hrt_absolute_time()); - - /* check if waypoint has been reached against the last positions */ - mavlink_waypoint_eventloop(hrt_absolute_time()); - if (lowspeed_counter % (50000 / MAIN_LOOP_DELAY) == 0) { /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); mavlink_waypoint_eventloop(hrt_absolute_time()); - mavlink_waypoint_eventloop(hrt_absolute_time()); - if (_baudrate > 57600) { mavlink_pm_queued_send(); } - /* send one string at 10 Hz */ - if (!mavlink_logbuffer_is_empty(&lb)) { + /* send one string at 20 Hz */ + if (!mavlink_logbuffer_is_empty(&lb)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&lb, &msg); if (lb_ret == OK) { mavlink_missionlib_send_gcs_string(msg.text); } - } + } } perf_end(_loop_perf); @@ -1636,7 +1623,6 @@ Mavlink::task_main(int argc, char *argv[]) /* wait for threads to complete */ pthread_join(receive_thread, NULL); - pthread_join(uorb_receive_thread, NULL); /* Reset the UART flags to original state */ tcsetattr(_uart, TCSANOW, &uart_config_original); @@ -1654,15 +1640,50 @@ Mavlink::task_main(int argc, char *argv[]) int Mavlink::start_helper(int argc, char *argv[]) { - // Create the instance in task context + /* create the instance in task context */ Mavlink *instance = Mavlink::new_instance(); - // This will actually only return once MAVLink exits + /* this will actually only return once MAVLink exits */ return instance->task_main(argc, argv); } int -Mavlink::start() +Mavlink::start(int argc, char *argv[]) { + // Instantiate thread + char buf[32]; + sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); + + /*mavlink->_mavlink_task = */task_spawn_cmd(buf, + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 4096, + (main_t)&Mavlink::start_helper, + (const char **)argv); + + // while (!this->is_running()) { + // usleep(200); + // } + + // if (mavlink->_mavlink_task < 0) { + // warn("task start failed"); + // return -errno; + // } + + // if (mavlink::g_mavlink != nullptr) { + // errx(1, "already running"); + // } + + // mavlink::g_mavlink = new Mavlink; + + // if (mavlink::g_mavlink == nullptr) { + // errx(1, "alloc failed"); + // } + + // if (OK != mavlink::g_mavlink->start()) { + // delete mavlink::g_mavlink; + // mavlink::g_mavlink = nullptr; + // err(1, "start failed"); + // } return OK; } @@ -1685,44 +1706,7 @@ int mavlink_main(int argc, char *argv[]) } if (!strcmp(argv[1], "start")) { - - // Instantiate thread - char buf[32]; - sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); - - /*mavlink->_mavlink_task = */task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); - - // while (!this->is_running()) { - // usleep(200); - // } - - // if (mavlink->_mavlink_task < 0) { - // warn("task start failed"); - // return -errno; - // } - - // if (mavlink::g_mavlink != nullptr) { - // errx(1, "already running"); - // } - - // mavlink::g_mavlink = new Mavlink; - - // if (mavlink::g_mavlink == nullptr) { - // errx(1, "alloc failed"); - // } - - // if (OK != mavlink::g_mavlink->start()) { - // delete mavlink::g_mavlink; - // mavlink::g_mavlink = nullptr; - // err(1, "start failed"); - // } - - return 0; + return Mavlink::start(argc, argv); } else if (!strcmp(argv[1], "stop")) { warnx("mavlink stop is deprecated, use stop-all instead"); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 56d262000..e7f3486da 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -142,7 +142,7 @@ public: * * @return OK on success. */ - static int start(); + static int start(int argc, char *argv[]); /** * Display the mavlink status. @@ -163,8 +163,6 @@ public: int get_uart_fd(); - int get_channel(); - const char *device_name; enum MAVLINK_MODE { @@ -205,16 +203,11 @@ public: MavlinkOrbSubscription *add_orb_subscription(const struct orb_metadata *topic, size_t size); - mavlink_channel_t get_chan() { return _chan; } + mavlink_channel_t get_channel(); bool _task_should_exit; /**< if true, mavlink task should exit */ protected: - /** - * Pointer to the default cdev file operations table; useful for - * registering clone devices etc. - */ - Mavlink* _next; private: @@ -234,7 +227,7 @@ private: orb_advert_t mission_pub; struct mission_s mission; - uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER + uint8_t missionlib_msg_buf[sizeof(mavlink_message_t)]; MAVLINK_MODE _mode; uint8_t _mavlink_wpm_comp_id; @@ -247,7 +240,6 @@ private: unsigned int total_counter; pthread_t receive_thread; - pthread_t uorb_receive_thread; /* Allocate storage space for waypoints */ mavlink_wpm_storage wpm_s; @@ -326,7 +318,7 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - int add_stream(const char *stream_name, const unsigned int interval); + int add_stream(const char *stream_name, const float rate); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7a56c36a5..df73581f0 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -12,195 +12,289 @@ #include "mavlink_messages.h" +class MavlinkStreamHeartbeat : public MavlinkStream { +public: + const char *get_name() + { + return "HEARTBEAT"; + } -struct msgs_list_s msgs_list[] = { - { - .name = "HEARTBEAT", - .callback = msg_heartbeat, - .topics = { ORB_ID(vehicle_status), ORB_ID(position_setpoint_triplet), nullptr }, - .sizes = { sizeof(struct vehicle_status_s), sizeof(struct position_setpoint_triplet_s) } - }, - { - .name = "SYS_STATUS", - .callback = msg_sys_status, - .topics = { ORB_ID(vehicle_status), nullptr }, - .sizes = { sizeof(struct vehicle_status_s) } - }, - { .name = nullptr } -}; + MavlinkStream *new_instance() + { + return new MavlinkStreamHeartbeat(); + } -void -msg_heartbeat(const MavlinkStream *stream) -{ - struct vehicle_status_s *status = (struct vehicle_status_s *)stream->subscriptions[0]->data; - struct position_setpoint_triplet_s *pos_sp_triplet = (struct position_setpoint_triplet_s *)stream->subscriptions[1]->data; +private: + MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *pos_sp_triplet_sub; - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; + struct vehicle_status_s *status; + struct position_setpoint_triplet_s *pos_sp_triplet; - /* HIL */ - if (status->hil_state == HIL_STATE_ON) { - mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; - } +protected: + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); + status = (struct vehicle_status_s *)status_sub->get_data(); - /* arming state */ - if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { - mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); + pos_sp_triplet = (struct position_setpoint_triplet_s *)status_sub->get_data(); } - /* main state */ - mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - union px4_custom_mode custom_mode; - custom_mode.data = 0; - if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { - /* use main state when navigator is not active */ - if (status->main_state == MAIN_STATE_MANUAL) { - mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_SEATBELT) { - mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; - } else if (status->main_state == MAIN_STATE_EASY) { - mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; - } else if (status->main_state == MAIN_STATE_AUTO) { + void send(const hrt_abstime t) { + status_sub->update(t); + pos_sp_triplet_sub->update(t); + + uint8_t mavlink_state = 0; + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + + /* HIL */ + if (status->hil_state == HIL_STATE_ON) { + mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + /* arming state */ + if (status->arming_state == ARMING_STATE_ARMED + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } + + /* main state */ + mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + union px4_custom_mode custom_mode; + custom_mode.data = 0; + if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { + /* use main state when navigator is not active */ + if (status->main_state == MAIN_STATE_MANUAL) { + mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (status->main_state == MAIN_STATE_SEATBELT) { + mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + } else if (status->main_state == MAIN_STATE_EASY) { + mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + } else if (status->main_state == MAIN_STATE_AUTO) { + mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } + } else { + /* use navigation state when navigator is active */ mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + if (pos_sp_triplet->nav_state == NAV_STATE_READY) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + } } - } else { - /* use navigation state when navigator is active */ - mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (pos_sp_triplet->nav_state == NAV_STATE_READY) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + mavlink_custom_mode = custom_mode.data; + + /* set system state */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + mavlink_state = MAV_STATE_UNINIT; + } else if (status->arming_state == ARMING_STATE_ARMED) { + mavlink_state = MAV_STATE_ACTIVE; + } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + mavlink_state = MAV_STATE_CRITICAL; + } else if (status->arming_state == ARMING_STATE_STANDBY) { + mavlink_state = MAV_STATE_STANDBY; + } else if (status->arming_state == ARMING_STATE_REBOOT) { + mavlink_state = MAV_STATE_POWEROFF; + } else { + mavlink_state = MAV_STATE_CRITICAL; } + + mavlink_msg_heartbeat_send(_channel, + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_base_mode, + mavlink_custom_mode, + mavlink_state); + } - mavlink_custom_mode = custom_mode.data; - - /* set system state */ - if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review - mavlink_state = MAV_STATE_UNINIT; - } else if (status->arming_state == ARMING_STATE_ARMED) { - mavlink_state = MAV_STATE_ACTIVE; - } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { - mavlink_state = MAV_STATE_CRITICAL; - } else if (status->arming_state == ARMING_STATE_STANDBY) { - mavlink_state = MAV_STATE_STANDBY; - } else if (status->arming_state == ARMING_STATE_REBOOT) { - mavlink_state = MAV_STATE_POWEROFF; - } else { - mavlink_state = MAV_STATE_CRITICAL; +}; + + +class MavlinkStreamSysStatus : public MavlinkStream { +public: + const char *get_name() + { + return "SYS_STATUS"; } - /* send heartbeat */ - mavlink_msg_heartbeat_send(stream->mavlink->get_chan(), - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); -} - -void -msg_sys_status(const MavlinkStream *stream) -{ - struct vehicle_status_s *status = (struct vehicle_status_s *)stream->subscriptions[0]->data; - - mavlink_msg_sys_status_send(stream->mavlink->get_chan(), - status->onboard_control_sensors_present, - status->onboard_control_sensors_enabled, - status->onboard_control_sensors_health, - status->load * 1000.0f, - status->battery_voltage * 1000.0f, - status->battery_current * 1000.0f, - status->battery_remaining, - status->drop_rate_comm, - status->errors_comm, - status->errors_count1, - status->errors_count2, - status->errors_count3, - status->errors_count4); -} - -void -msg_highres_imu(const MavlinkStream *stream) -{ - struct sensor_combined_s *sensor = (struct sensor_combined_s *)stream->subscriptions[0]->data; - - uint16_t fields_updated = 0; - - if (stream->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_highres_imu_send(stream->mavlink->get_chan(), sensor->timestamp, - sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], - sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], - sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], - sensor->baro_pres_mbar, sensor->differential_pressure_pa, - sensor->baro_alt_meter, sensor->baro_temp_celcius, - fields_updated); -} + MavlinkStream *new_instance() + { + return new MavlinkStreamSysStatus(); + } + +private: + MavlinkOrbSubscription *status_sub; + + struct vehicle_status_s *status; + +protected: + + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); + status = (struct vehicle_status_s *)status_sub->get_data(); + } + + void send(const hrt_abstime t) { + status_sub->update(t); + + mavlink_msg_sys_status_send(_channel, + status->onboard_control_sensors_present, + status->onboard_control_sensors_enabled, + status->onboard_control_sensors_health, + status->load * 1000.0f, + status->battery_voltage * 1000.0f, + status->battery_current * 1000.0f, + status->battery_remaining, + status->drop_rate_comm, + status->errors_comm, + status->errors_count1, + status->errors_count2, + status->errors_count3, + status->errors_count4); + } +}; + + +class MavlinkStreamHighresIMU : public MavlinkStream { +public: + const char *get_name() + { + return "HIGHRES_IMU"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamHighresIMU(); + } + +private: + MavlinkOrbSubscription *sensor_sub; + + struct sensor_combined_s *sensor; + + uint32_t accel_counter = 0; + uint32_t gyro_counter = 0; + uint32_t mag_counter = 0; + uint32_t baro_counter = 0; + +protected: + + void subscribe(Mavlink *mavlink) + { + sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined), sizeof(struct sensor_combined_s)); + sensor = (struct sensor_combined_s *)sensor_sub->get_data(); + } + + void send(const hrt_abstime t) { + sensor_sub->update(t); + + uint16_t fields_updated = 0; + + if (accel_counter != sensor->accelerometer_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + accel_counter = sensor->accelerometer_counter; + } + + if (gyro_counter != sensor->gyro_counter) { + /* mark second group dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_counter = sensor->gyro_counter; + } + + if (mag_counter != sensor->magnetometer_counter) { + /* mark third group dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_counter = sensor->magnetometer_counter; + } + + if (baro_counter != sensor->baro_counter) { + /* mark last group dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_counter = sensor->baro_counter; + } + + mavlink_msg_highres_imu_send(_channel, + sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); + } +}; + + +class MavlinkStreamAttitude : public MavlinkStream { +public: + const char *get_name() + { + return "ATTITUDE"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamAttitude(); + } + +private: + MavlinkOrbSubscription *att_sub; + + struct vehicle_attitude_s *att; + +protected: + + void subscribe(Mavlink *mavlink) + { + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); + att = (struct vehicle_attitude_s *)att_sub->get_data(); + } + + void send(const hrt_abstime t) { + att_sub->update(t); + + mavlink_msg_attitude_send(_channel, + att->timestamp / 1000, + att->roll, att->pitch, att->yaw, + att->rollspeed, att->pitchspeed, att->yawspeed); + } +}; + + +MavlinkStream *streams_list[] = { + new MavlinkStreamHeartbeat(), + new MavlinkStreamSysStatus(), + new MavlinkStreamHighresIMU(), + new MavlinkStreamAttitude(), + nullptr +}; + + + + + + + + -//void -//MavlinkOrbListener::l_sensor_combined(const struct listener *l) -//{ -// struct sensor_combined_s raw; -// -// /* copy sensors raw data into local buffer */ -// orb_copy(ORB_ID(sensor_combined), l->mavlink->get_subs()->sensor_sub, &raw); -// -// /* mark individual fields as _mavlink->get_chan()ged */ -// uint16_t fields_updated = 0; -// -// // if (accel_counter != raw.accelerometer_counter) { -// // /* mark first three dimensions as _mavlink->get_chan()ged */ -// // fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); -// // accel_counter = raw.accelerometer_counter; -// // } -// -// // if (gyro_counter != raw.gyro_counter) { -// // /* mark second group dimensions as _mavlink->get_chan()ged */ -// // fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); -// // gyro_counter = raw.gyro_counter; -// // } -// -// // if (mag_counter != raw.magnetometer_counter) { -// // /* mark third group dimensions as _mavlink->get_chan()ged */ -// // fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); -// // mag_counter = raw.magnetometer_counter; -// // } -// -// // if (baro_counter != raw.baro_counter) { -// // /* mark last group dimensions as _mavlink->get_chan()ged */ -// // fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); -// // baro_counter = raw.baro_counter; -// // } -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_highres_imu_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp, -// raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], -// raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], -// raw.gyro_rad_s[1], raw.gyro_rad_s[2], -// raw.magnetometer_ga[0], -// raw.magnetometer_ga[1], raw.magnetometer_ga[2], -// raw.baro_pres_mbar, raw.differential_pressure_pa, -// raw.baro_alt_meter, raw.baro_temp_celcius, -// fields_updated); -// -// l->listener->sensors_raw_counter++; -//} -// //void //MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) //{ diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index a6326bad1..3dc6cb699 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -10,19 +10,6 @@ #include "mavlink_stream.h" -#define MAX_TOPICS_PER_MAVLINK_STREAM 4 - -struct msgs_list_s { - char *name; - void (*callback)(const MavlinkStream *); - const struct orb_metadata *topics[MAX_TOPICS_PER_MAVLINK_STREAM+1]; - size_t sizes[MAX_TOPICS_PER_MAVLINK_STREAM+1]; -}; - -extern struct msgs_list_s msgs_list[]; - -static void msg_heartbeat(const MavlinkStream *stream); -static void msg_sys_status(const MavlinkStream *stream); -static void msg_highres_imu(const MavlinkStream *stream); +extern MavlinkStream *streams_list[]; #endif /* MAVLINK_MESSAGES_H_ */ diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 84ac11483..b504b6955 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -13,28 +13,40 @@ #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size) +MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size) : _topic(topic), _last_check(0), next(nullptr) { - this->topic = topic; - this->data = malloc(size); - memset(this->data, 0, size); - this->fd = orb_subscribe(topic); - this->last_update = 0; + _data = malloc(size); + memset(_data, 0, size); + _fd = orb_subscribe(_topic); } MavlinkOrbSubscription::~MavlinkOrbSubscription() { - close(fd); - free(data); + close(_fd); + free(_data); } -bool MavlinkOrbSubscription::update(const hrt_abstime t) +const struct orb_metadata * +MavlinkOrbSubscription::get_topic() { - if (last_update != t) { + return _topic; +} + +void * +MavlinkOrbSubscription::get_data() +{ + return _data; +} + +bool +MavlinkOrbSubscription::update(const hrt_abstime t) +{ + if (_last_check != t) { + _last_check = t; bool updated; - orb_check(fd, &updated); + orb_check(_fd, &updated); if (updated) { - orb_copy(topic, fd, data); + orb_copy(_topic, _fd, _data); return true; } } diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 9a7340e9b..c38a9cc43 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -11,18 +11,23 @@ #include #include + class MavlinkOrbSubscription { public: - MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size); + MavlinkOrbSubscription *next; + + MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size); ~MavlinkOrbSubscription(); bool update(const hrt_abstime t); + void *get_data(); + const struct orb_metadata *get_topic(); - const struct orb_metadata *topic; - int fd; - void *data; - hrt_abstime last_update; - MavlinkOrbSubscription *next; +private: + const struct orb_metadata *_topic; + int _fd; + void *_data; + hrt_abstime _last_check; }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d07de0f22..b828420e6 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -818,7 +818,7 @@ MavlinkReceiver::receive_thread(void *arg) /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { - if (mavlink_parse_char(_mavlink->get_chan(), buf[i], &msg, &status)) { + if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) { /* handle generic messages and commands */ handle_message(&msg); diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 9df4263ee..16407366e 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -10,39 +10,42 @@ #include "mavlink_stream.h" #include "mavlink_main.h" -MavlinkStream::MavlinkStream(Mavlink *mavlink, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **topics, const size_t *sizes, const uintptr_t arg, const unsigned int interval) +MavlinkStream::MavlinkStream() : _interval(1000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr) { - this->callback = callback; - this->arg = arg; - this->interval = interval * 1000; - this->mavlink = mavlink; - this->subscriptions_n = subs_n; - this->subscriptions = (MavlinkOrbSubscription **) malloc(subs_n * sizeof(MavlinkOrbSubscription *)); - - for (int i = 0; i < subs_n; i++) { - this->subscriptions[i] = mavlink->add_orb_subscription(topics[i], sizes[i]); - } } MavlinkStream::~MavlinkStream() { - free(subscriptions); } /** - * Update mavlink stream, i.e. update subscriptions and send message if necessary + * Set messages interval in ms */ -int MavlinkStream::update(const hrt_abstime t) +void +MavlinkStream::set_interval(const unsigned int interval) { - uint64_t dt = t - last_sent; - if (dt > 0 && dt >= interval) { - /* interval expired, update all subscriptions */ - for (unsigned int i = 0; i < subscriptions_n; i++) { - subscriptions[i]->update(t); - } + _interval = interval * 1000; +} - /* format and send mavlink message */ - callback(this); - last_sent = t; +/** + * Set mavlink channel + */ +void +MavlinkStream::set_channel(mavlink_channel_t channel) +{ + _channel = channel; +} + +/** + * Update subscriptions and send message if necessary + */ +int +MavlinkStream::update(const hrt_abstime t) +{ + uint64_t dt = t - _last_sent; + if (dt > 0 && dt >= _interval) { + /* interval expired, send message */ + send(t); + _last_sent = (t / _interval) * _interval; } } diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index c3e60917e..9f175adbe 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -15,22 +15,27 @@ class MavlinkStream; #include "mavlink_main.h" -class MavlinkOrbSubscription; - class MavlinkStream { +private: + hrt_abstime _last_sent; + +protected: + mavlink_channel_t _channel; + unsigned int _interval; + + virtual void send(const hrt_abstime t) = 0; + public: - void (*callback)(const MavlinkStream *); - uintptr_t arg; - unsigned int subscriptions_n; - MavlinkOrbSubscription **subscriptions; - hrt_abstime last_sent; - unsigned int interval; MavlinkStream *next; - Mavlink *mavlink; - MavlinkStream(Mavlink *mavlink, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **topics, const size_t *sizes, const uintptr_t arg, const unsigned int interval); + MavlinkStream(); ~MavlinkStream(); + void set_interval(const unsigned int interval); + void set_channel(mavlink_channel_t channel); int update(const hrt_abstime t); + virtual MavlinkStream *new_instance() = 0; + virtual void subscribe(Mavlink *mavlink) = 0; + virtual const char *get_name() = 0; }; -- cgit v1.2.3 From 2967763b160bb12b8e13e72219fcb689da015ab5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 22:13:49 +0400 Subject: mavlink: heartbeat message bug fixed --- src/modules/mavlink/mavlink_messages.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index df73581f0..0880e8285 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -38,7 +38,7 @@ protected: status = (struct vehicle_status_s *)status_sub->get_data(); pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); - pos_sp_triplet = (struct position_setpoint_triplet_s *)status_sub->get_data(); + pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } void send(const hrt_abstime t) { @@ -62,6 +62,7 @@ protected: /* main state */ mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + union px4_custom_mode custom_mode; custom_mode.data = 0; if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { @@ -96,7 +97,6 @@ protected: custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; } } - mavlink_custom_mode = custom_mode.data; /* set system state */ if (status->arming_state == ARMING_STATE_INIT @@ -119,7 +119,7 @@ protected: mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, - mavlink_custom_mode, + custom_mode.data, mavlink_state); } -- cgit v1.2.3 From 85dc422d9804c894009e37c6eaab67a10c5dca28 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 22:47:19 +0400 Subject: mavlink: more streams implemented, stack size returned to 2048 --- src/modules/mavlink/mavlink_main.cpp | 26 ++-- src/modules/mavlink/mavlink_messages.cpp | 204 +++++++++++++++++++++++-------- 2 files changed, 164 insertions(+), 66 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 33d81729c..5ac76e1cf 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -61,14 +61,6 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include #include #include @@ -79,6 +71,10 @@ #include #include +#include +#include +#include + #include "mavlink_bridge_header.h" #include "mavlink_main.h" #include "mavlink_messages.h" @@ -1542,13 +1538,11 @@ Mavlink::task_main(int argc, char *argv[]) add_stream("HEARTBEAT", 1.0f); add_stream("SYS_STATUS", 1.0f); - add_stream("HIGHRES_IMU", 20.0f); -// add_stream("RAW_IMU", 10.0f); - add_stream("ATTITUDE", 20.0f); -// add_stream("NAMED_VALUE_FLOAT", 5.0f); -// add_stream("SERVO_OUTPUT_RAW", 2.0f); -// add_stream("GPS_RAW_INT", 2.0f); -// add_stream("MANUAL_CONTROL", 2.0f); + add_stream("HIGHRES_IMU", 1.0f); + add_stream("ATTITUDE", 10.0f); + add_stream("GPS_RAW_INT", 1.0f); + add_stream("GLOBAL_POSITION_INT", 5.0f); + add_stream("LOCAL_POSITION_NED", 5.0f); while (!_task_should_exit) { /* main loop */ @@ -1656,7 +1650,7 @@ Mavlink::start(int argc, char *argv[]) /*mavlink->_mavlink_task = */task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 4096, + 2048, (main_t)&Mavlink::start_helper, (const char **)argv); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 0880e8285..27ae197a6 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -6,12 +6,32 @@ */ #include +#include +#include +#include +#include #include #include +#include +#include #include "mavlink_messages.h" + +uint16_t +cm_uint16_from_m_float(float m) +{ + if (m < 0.0f) { + return 0; + + } else if (m > 655.35f) { + return 65535; + } + + return (uint16_t)(m * 100.0f); +} + class MavlinkStreamHeartbeat : public MavlinkStream { public: const char *get_name() @@ -279,11 +299,145 @@ protected: }; +class MavlinkStreamGPSRawInt : public MavlinkStream { +public: + const char *get_name() + { + return "GPS_RAW_INT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamGPSRawInt(); + } + +private: + MavlinkOrbSubscription *gps_sub; + + struct vehicle_gps_position_s *gps; + +protected: + + void subscribe(Mavlink *mavlink) + { + gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position), sizeof(struct vehicle_gps_position_s)); + gps = (struct vehicle_gps_position_s *)gps_sub->get_data(); + } + + void send(const hrt_abstime t) { + gps_sub->update(t); + + mavlink_msg_gps_raw_int_send(_channel, + gps->timestamp_position, + gps->fix_type, + gps->lat, + gps->lon, + gps->alt, + cm_uint16_from_m_float(gps->eph_m), + cm_uint16_from_m_float(gps->epv_m), + gps->vel_m_s * 100.0f, + _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps->satellites_visible); + } +}; + + +class MavlinkStreamGlobalPositionInt : public MavlinkStream { +public: + const char *get_name() + { + return "GLOBAL_POSITION_INT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamGlobalPositionInt(); + } + +private: + MavlinkOrbSubscription *pos_sub; + MavlinkOrbSubscription *home_sub; + + struct vehicle_global_position_s *pos; + struct home_position_s *home; + +protected: + + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s)); + pos = (struct vehicle_global_position_s *)pos_sub->get_data(); + + home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s)); + home = (struct home_position_s *)home_sub->get_data(); + } + + void send(const hrt_abstime t) { + pos_sub->update(t); + home_sub->update(t); + + mavlink_msg_global_position_int_send(_channel, + pos->timestamp / 1000, + pos->lat * 1e7, + pos->lon * 1e7, + pos->alt * 1000.0f, + (pos->alt - home->alt) * 1000.0f, + pos->vel_n * 100.0f, + pos->vel_e * 100.0f, + pos->vel_d * 100.0f, + _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + } +}; + + +class MavlinkStreamLocalPositionNED : public MavlinkStream { +public: + const char *get_name() + { + return "LOCAL_POSITION_NED"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamLocalPositionNED(); + } + +private: + MavlinkOrbSubscription *pos_sub; + + struct vehicle_local_position_s *pos; + +protected: + + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position), sizeof(struct vehicle_local_position_s)); + pos = (struct vehicle_local_position_s *)pos_sub->get_data(); + } + + void send(const hrt_abstime t) { + pos_sub->update(t); + + mavlink_msg_local_position_ned_send(_channel, + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->vx, + pos->vy, + pos->vz); + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), new MavlinkStreamHighresIMU(), new MavlinkStreamAttitude(), + new MavlinkStreamGPSRawInt(), + new MavlinkStreamGlobalPositionInt(), + new MavlinkStreamLocalPositionNED(), nullptr }; @@ -347,22 +501,6 @@ MavlinkStream *streams_list[] = { // /* copy gps data into local buffer */ // orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_subs()->gps_sub, &gps); // -// /* GPS COG is 0..2PI in degrees * 1e2 */ -// float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F; -// -// /* GPS position */ -// mavlink_msg_gps_raw_int_send(l->mavlink->get_chan(), -// gps.timestamp_position, -// gps.fix_type, -// gps.lat, -// gps.lon, -// gps.alt, -// cm_uint16_from_m_float(gps.eph_m), -// cm_uint16_from_m_float(gps.epv_m), -// gps.vel_m_s * 1e2f, // from m/s to cm/s -// cog_deg * 1e2f, // from deg to deg * 100 -// gps.satellites_visible); -// // /* update SAT info every 10 seconds */ // if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) { // mavlink_msg_gps_status_send(l->mavlink->get_chan(), @@ -414,40 +552,6 @@ MavlinkStream *streams_list[] = { // } //} // -//void -//MavlinkOrbListener::l_global_position(const struct listener *l) -//{ -// /* copy global position data into local buffer */ -// orb_copy(ORB_ID(vehicle_global_position), l->mavlink->get_subs()->global_pos_sub, &l->listener->global_pos); -// -// mavlink_msg_global_position_int_send(l->mavlink->get_chan(), -// l->listener->global_pos.timestamp / 1000, -// l->listener->global_pos.lat * 1e7, -// l->listener->global_pos.lon * 1e7, -// l->listener->global_pos.alt * 1000.0f, -// (l->listener->global_pos.alt - l->listener->home.alt) * 1000.0f, -// l->listener->global_pos.vel_n * 100.0f, -// l->listener->global_pos.vel_e * 100.0f, -// l->listener->global_pos.vel_d * 100.0f, -// _wrap_2pi(l->listener->global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); -//} -// -//void -//MavlinkOrbListener::l_local_position(const struct listener *l) -//{ -// /* copy local position data into local buffer */ -// orb_copy(ORB_ID(vehicle_local_position), l->mavlink->get_subs()->local_pos_sub, &l->listener->local_pos); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_local_position_ned_send(l->mavlink->get_chan(), -// l->listener->local_pos.timestamp / 1000, -// l->listener->local_pos.x, -// l->listener->local_pos.y, -// l->listener->local_pos.z, -// l->listener->local_pos.vx, -// l->listener->local_pos.vy, -// l->listener->local_pos.vz); -//} // //void //MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) -- cgit v1.2.3 From 18f72f8dd7a3bd4a9af00fe28afd29e0cc65e2e9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 23:02:53 +0400 Subject: mavlink: GPS_GLOBAL_ORIGIN message added, set message rate depending on baudrate --- src/modules/mavlink/mavlink_main.cpp | 21 +++++++------ src/modules/mavlink/mavlink_messages.cpp | 53 ++++++++++++++++++++++---------- 2 files changed, 48 insertions(+), 26 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5ac76e1cf..3102c937d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1531,18 +1531,19 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_log_info(_mavlink_fd, "[mavlink] started"); /* add default streams, intervals depend on baud rate */ -// if (_baudrate >= 230400) { -// } else if (_baudrate >= 115200) { -// } else if (_baudrate >= 57600) { -// } + float rate_mult = _baudrate / 57600.0f; + if (rate_mult > 4.0f) { + rate_mult = 4.0f; + } add_stream("HEARTBEAT", 1.0f); - add_stream("SYS_STATUS", 1.0f); - add_stream("HIGHRES_IMU", 1.0f); - add_stream("ATTITUDE", 10.0f); - add_stream("GPS_RAW_INT", 1.0f); - add_stream("GLOBAL_POSITION_INT", 5.0f); - add_stream("LOCAL_POSITION_NED", 5.0f); + add_stream("SYS_STATUS", 1.0f * rate_mult); + add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + add_stream("HIGHRES_IMU", 1.0f * rate_mult); + add_stream("ATTITUDE", 10.0f * rate_mult); + add_stream("GPS_RAW_INT", 1.0f * rate_mult); + add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); + add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); while (!_task_should_exit) { /* main loop */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 27ae197a6..48443fbdc 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -430,6 +430,42 @@ protected: }; +class MavlinkStreamGPSGlobalOrigin : public MavlinkStream { +public: + const char *get_name() + { + return "GPS_GLOBAL_ORIGIN"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamGPSGlobalOrigin(); + } + +private: + MavlinkOrbSubscription *home_sub; + + struct home_position_s *home; + +protected: + + void subscribe(Mavlink *mavlink) + { + home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s)); + home = (struct home_position_s *)home_sub->get_data(); + } + + void send(const hrt_abstime t) { + home_sub->update(t); + + mavlink_msg_gps_global_origin_send(_channel, + (int32_t)(home->lat * 1e7), + (int32_t)(home->lon * 1e7), + (int32_t)(home->alt) * 1000.0f); + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -438,6 +474,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamGPSRawInt(), new MavlinkStreamGlobalPositionInt(), new MavlinkStreamLocalPositionNED(), + new MavlinkStreamGPSGlobalOrigin(), nullptr }; @@ -517,14 +554,6 @@ MavlinkStream *streams_list[] = { // // //void -//MavlinkOrbListener::l_rc_channels(const struct listener *l) -//{ -// /* copy rc channels into local buffer */ -// orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc); -// // XXX Add RC channels scaled message here -//} -// -//void //MavlinkOrbListener::l_input_rc(const struct listener *l) //{ // /* copy rc _mavlink->get_chan()nels into local buffer */ @@ -801,14 +830,6 @@ MavlinkStream *streams_list[] = { //} // //void -//MavlinkOrbListener::l_home(const struct listener *l) -//{ -// orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home); -// -// mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f); -//} -// -//void //MavlinkOrbListener::l_airspeed(const struct listener *l) //{ // orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed); -- cgit v1.2.3 From 141982a3ac21e7a0437f1d7692e4024daf873c21 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 27 Feb 2014 13:54:55 +0400 Subject: mavlink: minor refactoring and cleanup, rate limiter class implemented, new messages added --- src/modules/mavlink/mavlink_main.cpp | 68 ++-- src/modules/mavlink/mavlink_main.h | 9 +- src/modules/mavlink/mavlink_messages.cpp | 438 +++++++++++++---------- src/modules/mavlink/mavlink_orb_subscription.cpp | 2 +- src/modules/mavlink/mavlink_orb_subscription.h | 4 +- src/modules/mavlink/mavlink_rate_limiter.cpp | 38 ++ src/modules/mavlink/mavlink_rate_limiter.h | 28 ++ src/modules/mavlink/mavlink_stream.cpp | 2 +- src/modules/mavlink/module.mk | 3 +- 9 files changed, 371 insertions(+), 221 deletions(-) create mode 100644 src/modules/mavlink/mavlink_rate_limiter.cpp create mode 100644 src/modules/mavlink/mavlink_rate_limiter.h (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3102c937d..8a026742c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -79,6 +79,7 @@ #include "mavlink_main.h" #include "mavlink_messages.h" #include "mavlink_receiver.h" +#include "mavlink_rate_limiter.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -321,7 +322,7 @@ Mavlink::get_uart_fd() mavlink_channel_t Mavlink::get_channel() { - return _chan; + return _channel; } /**************************************************************************** @@ -1321,7 +1322,7 @@ Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) { uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); - mavlink_send_uart_bytes(_chan, missionlib_msg_buf, len); + mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len); } @@ -1356,7 +1357,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) } } -MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata *topic, const size_t size) +MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, const size_t size) { /* check if already subscribed to this topic */ MavlinkOrbSubscription *sub; @@ -1406,7 +1407,7 @@ Mavlink::task_main(int argc, char *argv[]) int ch; _baudrate = 57600; - _chan = MAVLINK_COMM_0; + _channel = MAVLINK_COMM_0; _mode = MODE_OFFBOARD; @@ -1520,8 +1521,6 @@ Mavlink::task_main(int argc, char *argv[]) thread_running = true; - unsigned lowspeed_counter = 0; - MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s)); MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s)); @@ -1530,20 +1529,42 @@ Mavlink::task_main(int argc, char *argv[]) warnx("started"); mavlink_log_info(_mavlink_fd, "[mavlink] started"); - /* add default streams, intervals depend on baud rate */ + /* add default streams depending on mode, intervals depend on baud rate */ float rate_mult = _baudrate / 57600.0f; if (rate_mult > 4.0f) { rate_mult = 4.0f; } add_stream("HEARTBEAT", 1.0f); - add_stream("SYS_STATUS", 1.0f * rate_mult); - add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); - add_stream("HIGHRES_IMU", 1.0f * rate_mult); - add_stream("ATTITUDE", 10.0f * rate_mult); - add_stream("GPS_RAW_INT", 1.0f * rate_mult); - add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); - add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); + + switch(_mode) { + case MODE_OFFBOARD: + add_stream("SYS_STATUS", 1.0f * rate_mult); + add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + add_stream("HIGHRES_IMU", 1.0f * rate_mult); + add_stream("ATTITUDE", 10.0f * rate_mult); + add_stream("GPS_RAW_INT", 1.0f * rate_mult); + add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); + add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); + add_stream("SERVO_OUTPUT_RAW_0", 1.0f * rate_mult); + break; + + case MODE_HIL: + add_stream("SYS_STATUS", 1.0f * rate_mult); + add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + add_stream("HIGHRES_IMU", 1.0f * rate_mult); + add_stream("ATTITUDE", 10.0f * rate_mult); + add_stream("GPS_RAW_INT", 1.0f * rate_mult); + add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); + add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); + break; + + default: + break; + } + + MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult); + MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult); while (!_task_should_exit) { /* main loop */ @@ -1571,13 +1592,6 @@ Mavlink::task_main(int argc, char *argv[]) stream->update(t); } - /* 0.5 Hz */ - if (lowspeed_counter % (2000000 / MAIN_LOOP_DELAY) == 0) { - mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); - } - - lowspeed_counter++; - bool updated; orb_check(mission_result_sub, &updated); @@ -1591,18 +1605,16 @@ Mavlink::task_main(int argc, char *argv[]) } mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); + } else { + if (slow_rate_limiter.check(t)) { + mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); + } } - if (lowspeed_counter % (50000 / MAIN_LOOP_DELAY) == 0) { - /* send parameters at 20 Hz (if queued for sending) */ + if (fast_rate_limiter.check(t)) { mavlink_pm_queued_send(); mavlink_waypoint_eventloop(hrt_absolute_time()); - if (_baudrate > 57600) { - mavlink_pm_queued_send(); - } - - /* send one string at 20 Hz */ if (!mavlink_logbuffer_is_empty(&lb)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&lb, &msg); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e7f3486da..506b4317a 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -201,7 +201,7 @@ public: */ int set_hil_enabled(bool hil_enabled); - MavlinkOrbSubscription *add_orb_subscription(const struct orb_metadata *topic, size_t size); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, size_t size); mavlink_channel_t get_channel(); @@ -231,10 +231,7 @@ private: MAVLINK_MODE _mode; uint8_t _mavlink_wpm_comp_id; - mavlink_channel_t _chan; - -// XXX probably should be in a header... -// extern pthread_t receive_start(int uart); + mavlink_channel_t _channel; struct mavlink_logbuffer lb; unsigned int total_counter; @@ -248,7 +245,7 @@ private: bool _verbose; int _uart; int _baudrate; - bool gcs_link; + /** * If the queue index is not at 0, the queue sending * logic will send parameters from the current index diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 48443fbdc..967606841 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -5,6 +5,7 @@ * Author: ton */ +#include #include #include @@ -19,6 +20,10 @@ #include "mavlink_messages.h" +static uint16_t cm_uint16_from_m_float(float m); +static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + uint16_t cm_uint16_from_m_float(float m) { @@ -32,6 +37,83 @@ cm_uint16_from_m_float(float m) return (uint16_t)(m * 100.0f); } +void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) +{ + *mavlink_state = 0; + *mavlink_base_mode = 0; + *mavlink_custom_mode = 0; + + /* HIL */ + if (status->hil_state == HIL_STATE_ON) { + *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + /* arming state */ + if (status->arming_state == ARMING_STATE_ARMED + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } + + /* main state */ + *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + + union px4_custom_mode custom_mode; + custom_mode.data = 0; + if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { + /* use main state when navigator is not active */ + if (status->main_state == MAIN_STATE_MANUAL) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (status->main_state == MAIN_STATE_SEATBELT) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + } else if (status->main_state == MAIN_STATE_EASY) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + } else if (status->main_state == MAIN_STATE_AUTO) { + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } + } else { + /* use navigation state when navigator is active */ + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + if (pos_sp_triplet->nav_state == NAV_STATE_READY) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + } + } + + *mavlink_custom_mode = custom_mode.data; + + /* set system state */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + *mavlink_state = MAV_STATE_UNINIT; + } else if (status->arming_state == ARMING_STATE_ARMED) { + *mavlink_state = MAV_STATE_ACTIVE; + } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + *mavlink_state = MAV_STATE_CRITICAL; + } else if (status->arming_state == ARMING_STATE_STANDBY) { + *mavlink_state = MAV_STATE_STANDBY; + } else if (status->arming_state == ARMING_STATE_REBOOT) { + *mavlink_state = MAV_STATE_POWEROFF; + } else { + *mavlink_state = MAV_STATE_CRITICAL; + } +} + + class MavlinkStreamHeartbeat : public MavlinkStream { public: const char *get_name() @@ -68,78 +150,13 @@ protected: uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; uint32_t mavlink_custom_mode = 0; - - /* HIL */ - if (status->hil_state == HIL_STATE_ON) { - mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; - } - - /* arming state */ - if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { - mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; - } - - /* main state */ - mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - - union px4_custom_mode custom_mode; - custom_mode.data = 0; - if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { - /* use main state when navigator is not active */ - if (status->main_state == MAIN_STATE_MANUAL) { - mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_SEATBELT) { - mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; - } else if (status->main_state == MAIN_STATE_EASY) { - mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; - } else if (status->main_state == MAIN_STATE_AUTO) { - mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } - } else { - /* use navigation state when navigator is active */ - mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (pos_sp_triplet->nav_state == NAV_STATE_READY) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; - } - } - - /* set system state */ - if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review - mavlink_state = MAV_STATE_UNINIT; - } else if (status->arming_state == ARMING_STATE_ARMED) { - mavlink_state = MAV_STATE_ACTIVE; - } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { - mavlink_state = MAV_STATE_CRITICAL; - } else if (status->arming_state == ARMING_STATE_STANDBY) { - mavlink_state = MAV_STATE_STANDBY; - } else if (status->arming_state == ARMING_STATE_REBOOT) { - mavlink_state = MAV_STATE_POWEROFF; - } else { - mavlink_state = MAV_STATE_CRITICAL; - } + get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); mavlink_msg_heartbeat_send(_channel, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, - custom_mode.data, + mavlink_custom_mode, mavlink_state); } @@ -164,7 +181,6 @@ private: struct vehicle_status_s *status; protected: - void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); @@ -215,7 +231,6 @@ private: uint32_t baro_counter = 0; protected: - void subscribe(Mavlink *mavlink) { sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined), sizeof(struct sensor_combined_s)); @@ -281,7 +296,6 @@ private: struct vehicle_attitude_s *att; protected: - void subscribe(Mavlink *mavlink) { att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); @@ -313,11 +327,9 @@ public: private: MavlinkOrbSubscription *gps_sub; - struct vehicle_gps_position_s *gps; protected: - void subscribe(Mavlink *mavlink) { gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position), sizeof(struct vehicle_gps_position_s)); @@ -356,13 +368,12 @@ public: private: MavlinkOrbSubscription *pos_sub; - MavlinkOrbSubscription *home_sub; - struct vehicle_global_position_s *pos; + + MavlinkOrbSubscription *home_sub; struct home_position_s *home; protected: - void subscribe(Mavlink *mavlink) { pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s)); @@ -404,11 +415,9 @@ public: private: MavlinkOrbSubscription *pos_sub; - struct vehicle_local_position_s *pos; protected: - void subscribe(Mavlink *mavlink) { pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position), sizeof(struct vehicle_local_position_s)); @@ -444,11 +453,9 @@ public: private: MavlinkOrbSubscription *home_sub; - struct home_position_s *home; protected: - void subscribe(Mavlink *mavlink) { home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s)); @@ -466,6 +473,171 @@ protected: }; +class MavlinkStreamServoOutputRaw : public MavlinkStream { +public: + MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n) + { + sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n); + } + + const char *get_name() + { + return _name; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamServoOutputRaw(_n); + } + +private: + MavlinkOrbSubscription *act_sub; + struct actuator_outputs_s *act; + + char _name[20]; + unsigned int _n; + +protected: + void subscribe(Mavlink *mavlink) + { + orb_id_t act_topics[] = { + ORB_ID(actuator_outputs_0), + ORB_ID(actuator_outputs_1), + ORB_ID(actuator_outputs_2), + ORB_ID(actuator_outputs_3) + }; + + act_sub = mavlink->add_orb_subscription(act_topics[_n], sizeof(struct actuator_outputs_s)); + act = (struct actuator_outputs_s *)act_sub->get_data(); + } + + void send(const hrt_abstime t) { + act_sub->update(t); + + mavlink_msg_servo_output_raw_send(_channel, + act->timestamp / 1000, + _n, + act->output[0], + act->output[1], + act->output[2], + act->output[3], + act->output[4], + act->output[5], + act->output[6], + act->output[7]); + } +}; + + +class MavlinkStreamHILControls : public MavlinkStream { +public: + const char *get_name() + { + return "HIL_CONTROLS"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamHILControls(); + } + +private: + MavlinkOrbSubscription *status_sub; + struct vehicle_status_s *status; + + MavlinkOrbSubscription *pos_sp_triplet_sub; + struct position_setpoint_triplet_s *pos_sp_triplet; + + MavlinkOrbSubscription *act_sub; + struct actuator_outputs_s *act; + +protected: + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); + status = (struct vehicle_status_s *)status_sub->get_data(); + + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); + pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); + + act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0), sizeof(struct actuator_outputs_s)); + act = (struct actuator_outputs_s *)act_sub->get_data(); + } + + void send(const hrt_abstime t) { + status_sub->update(t); + pos_sp_triplet_sub->update(t); + act_sub->update(t); + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state; + uint8_t mavlink_base_mode; + uint32_t mavlink_custom_mode; + get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + + /* HIL message as per MAVLink spec */ + + /* scale / assign outputs depending on system type */ + if (mavlink_system.type == MAV_TYPE_QUADROTOR) { + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + ((act->output[0] - 900.0f) / 600.0f) / 2.0f, + ((act->output[1] - 900.0f) / 600.0f) / 2.0f, + ((act->output[2] - 900.0f) / 600.0f) / 2.0f, + ((act->output[3] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + -1, + -1, + mavlink_base_mode, + 0); + + } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + ((act->output[0] - 900.0f) / 600.0f) / 2.0f, + ((act->output[1] - 900.0f) / 600.0f) / 2.0f, + ((act->output[2] - 900.0f) / 600.0f) / 2.0f, + ((act->output[3] - 900.0f) / 600.0f) / 2.0f, + ((act->output[4] - 900.0f) / 600.0f) / 2.0f, + ((act->output[5] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + mavlink_base_mode, + 0); + + } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + ((act->output[0] - 900.0f) / 600.0f) / 2.0f, + ((act->output[1] - 900.0f) / 600.0f) / 2.0f, + ((act->output[2] - 900.0f) / 600.0f) / 2.0f, + ((act->output[3] - 900.0f) / 600.0f) / 2.0f, + ((act->output[4] - 900.0f) / 600.0f) / 2.0f, + ((act->output[5] - 900.0f) / 600.0f) / 2.0f, + ((act->output[6] - 900.0f) / 600.0f) / 2.0f, + ((act->output[7] - 900.0f) / 600.0f) / 2.0f, + mavlink_base_mode, + 0); + + } else { + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + (act->output[0] - 1500.0f) / 500.0f, + (act->output[1] - 1500.0f) / 500.0f, + (act->output[2] - 1500.0f) / 500.0f, + (act->output[3] - 1000.0f) / 1000.0f, + (act->output[4] - 1500.0f) / 500.0f, + (act->output[5] - 1500.0f) / 500.0f, + (act->output[6] - 1500.0f) / 500.0f, + (act->output[7] - 1500.0f) / 500.0f, + mavlink_base_mode, + 0); + } + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -475,6 +647,11 @@ MavlinkStream *streams_list[] = { new MavlinkStreamGlobalPositionInt(), new MavlinkStreamLocalPositionNED(), new MavlinkStreamGPSGlobalOrigin(), + new MavlinkStreamServoOutputRaw(0), + new MavlinkStreamServoOutputRaw(1), + new MavlinkStreamServoOutputRaw(2), + new MavlinkStreamServoOutputRaw(3), + new MavlinkStreamHILControls(), nullptr }; @@ -484,8 +661,6 @@ MavlinkStream *streams_list[] = { - - //void //MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) //{ @@ -652,107 +827,6 @@ MavlinkStream *streams_list[] = { //} // //void -//MavlinkOrbListener::l_actuator_outputs(const struct listener *l) -//{ -// struct actuator_outputs_s act_outputs; -// -// orb_id_t ids[] = { -// ORB_ID(actuator_outputs_0), -// ORB_ID(actuator_outputs_1), -// ORB_ID(actuator_outputs_2), -// ORB_ID(actuator_outputs_3) -// }; -// -// /* copy actuator data into local buffer */ -// orb_copy(ids[l->arg], *l->subp, &act_outputs); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { -// mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp / 1000, -// l->arg /* port number - needs GCS support */, -// /* QGC has port number support already */ -// act_outputs.output[0], -// act_outputs.output[1], -// act_outputs.output[2], -// act_outputs.output[3], -// act_outputs.output[4], -// act_outputs.output[5], -// act_outputs.output[6], -// act_outputs.output[7]); -// -// /* only send in HIL mode and only send first group for HIL */ -// if (l->mavlink->get_hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { -// -// /* translate the current syste state to mavlink state and mode */ -// uint8_t mavlink_state = 0; -// uint8_t mavlink_base_mode = 0; -// uint32_t mavlink_custom_mode = 0; -// l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); -// -// /* HIL message as per MAVLink spec */ -// -// /* scale / assign outputs depending on system type */ -// -// if (mavlink_system.type == MAV_TYPE_QUADROTOR) { -// mavlink_msg_hil_controls_send(l->mavlink->get_chan(), -// hrt_absolute_time(), -// ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, -// -1, -// -1, -// -1, -// -1, -// mavlink_base_mode, -// 0); -// -// } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { -// mavlink_msg_hil_controls_send(l->mavlink->get_chan(), -// hrt_absolute_time(), -// ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, -// -1, -// -1, -// mavlink_base_mode, -// 0); -// -// } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { -// mavlink_msg_hil_controls_send(l->mavlink->get_chan(), -// hrt_absolute_time(), -// ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, -// mavlink_base_mode, -// 0); -// -// } else { -// mavlink_msg_hil_controls_send(l->mavlink->get_chan(), -// hrt_absolute_time(), -// (act_outputs.output[0] - 1500.0f) / 500.0f, -// (act_outputs.output[1] - 1500.0f) / 500.0f, -// (act_outputs.output[2] - 1500.0f) / 500.0f, -// (act_outputs.output[3] - 1000.0f) / 1000.0f, -// (act_outputs.output[4] - 1500.0f) / 500.0f, -// (act_outputs.output[5] - 1500.0f) / 500.0f, -// (act_outputs.output[6] - 1500.0f) / 500.0f, -// (act_outputs.output[7] - 1500.0f) / 500.0f, -// mavlink_base_mode, -// 0); -// } -// } -// } -//} -// -//void //MavlinkOrbListener::l_actuator_armed(const struct listener *l) //{ // orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index b504b6955..35470a19a 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -13,7 +13,7 @@ #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size) : _topic(topic), _last_check(0), next(nullptr) +MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, size_t size) : _topic(topic), _last_check(0), next(nullptr) { _data = malloc(size); memset(_data, 0, size); diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index c38a9cc43..79ff3abdb 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -16,7 +16,7 @@ class MavlinkOrbSubscription { public: MavlinkOrbSubscription *next; - MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size); + MavlinkOrbSubscription(const orb_id_t topic, size_t size); ~MavlinkOrbSubscription(); bool update(const hrt_abstime t); @@ -24,7 +24,7 @@ public: const struct orb_metadata *get_topic(); private: - const struct orb_metadata *_topic; + const orb_id_t _topic; int _fd; void *_data; hrt_abstime _last_check; diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp new file mode 100644 index 000000000..f5bb06ccd --- /dev/null +++ b/src/modules/mavlink/mavlink_rate_limiter.cpp @@ -0,0 +1,38 @@ +/* + * mavlink_rate_limiter.cpp + * + * Created on: 26.02.2014 + * Author: ton + */ + + +#include "mavlink_rate_limiter.h" + +MavlinkRateLimiter::MavlinkRateLimiter() : _last_sent(0), _interval(1000000) +{ +} + +MavlinkRateLimiter::MavlinkRateLimiter(unsigned int interval) : _last_sent(0), _interval(interval * 1000) +{ +} + +MavlinkRateLimiter::~MavlinkRateLimiter() +{ +} + +void +MavlinkRateLimiter::set_interval(unsigned int interval) +{ + _interval = interval * 1000; +} + +bool +MavlinkRateLimiter::check(hrt_abstime t) +{ + uint64_t dt = t - _last_sent; + if (dt > 0 && dt >= _interval) { + _last_sent = (t / _interval) * _interval; + return true; + } + return false; +} diff --git a/src/modules/mavlink/mavlink_rate_limiter.h b/src/modules/mavlink/mavlink_rate_limiter.h new file mode 100644 index 000000000..6db65f638 --- /dev/null +++ b/src/modules/mavlink/mavlink_rate_limiter.h @@ -0,0 +1,28 @@ +/* + * mavlink_rate_limiter.h + * + * Created on: 26.02.2014 + * Author: ton + */ + +#ifndef MAVLINK_RATE_LIMITER_H_ +#define MAVLINK_RATE_LIMITER_H_ + +#include + + +class MavlinkRateLimiter { +private: + hrt_abstime _last_sent; + hrt_abstime _interval; + +public: + MavlinkRateLimiter(); + MavlinkRateLimiter(unsigned int interval); + ~MavlinkRateLimiter(); + void set_interval(unsigned int interval); + bool check(hrt_abstime t); +}; + + +#endif /* MAVLINK_RATE_LIMITER_H_ */ diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 16407366e..703f74b4c 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -10,7 +10,7 @@ #include "mavlink_stream.h" #include "mavlink_main.h" -MavlinkStream::MavlinkStream() : _interval(1000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr) +MavlinkStream::MavlinkStream() : _interval(1000000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr) { } diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 5ea6c0f46..f09efa2e6 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -41,6 +41,7 @@ SRCS += mavlink_main.cpp \ mavlink_receiver.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ - mavlink_stream.cpp + mavlink_stream.cpp \ + mavlink_rate_limiter.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink -- cgit v1.2.3 From 323b90bfd9f3a1524f36c089d532ed3836cc2ea3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Feb 2014 23:44:51 +0400 Subject: mavlink: uORB topics includes moved to mavlink_messages.cpp, more messages implemented --- src/modules/mavlink/mavlink_main.h | 28 +- src/modules/mavlink/mavlink_messages.cpp | 433 +++++++++++++++++++++---------- 2 files changed, 300 insertions(+), 161 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 049f5fedd..41e781ee8 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -42,44 +42,18 @@ #include #include -#include #include #include #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include "mavlink_bridge_header.h" #include "mavlink_orb_subscription.h" #include "mavlink_stream.h" +#include "mavlink_messages.h" // FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 967606841..3634acefa 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -10,12 +10,33 @@ #include #include +#include +#include +#include +#include #include +#include #include #include +#include +#include +#include +#include +#include +#include #include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "mavlink_messages.h" @@ -128,9 +149,9 @@ public: private: MavlinkOrbSubscription *status_sub; - MavlinkOrbSubscription *pos_sp_triplet_sub; - struct vehicle_status_s *status; + + MavlinkOrbSubscription *pos_sp_triplet_sub; struct position_setpoint_triplet_s *pos_sp_triplet; protected: @@ -177,7 +198,6 @@ public: private: MavlinkOrbSubscription *status_sub; - struct vehicle_status_s *status; protected: @@ -210,6 +230,10 @@ protected: class MavlinkStreamHighresIMU : public MavlinkStream { public: + MavlinkStreamHighresIMU() : MavlinkStream(), accel_counter(0), gyro_counter(0), mag_counter(0), baro_counter(0) + { + } + const char *get_name() { return "HIGHRES_IMU"; @@ -222,13 +246,12 @@ public: private: MavlinkOrbSubscription *sensor_sub; - struct sensor_combined_s *sensor; - uint32_t accel_counter = 0; - uint32_t gyro_counter = 0; - uint32_t mag_counter = 0; - uint32_t baro_counter = 0; + uint32_t accel_counter; + uint32_t gyro_counter; + uint32_t mag_counter; + uint32_t baro_counter; protected: void subscribe(Mavlink *mavlink) @@ -292,7 +315,6 @@ public: private: MavlinkOrbSubscription *att_sub; - struct vehicle_attitude_s *att; protected: @@ -313,6 +335,75 @@ protected: }; +class MavlinkStreamVFRHUD : public MavlinkStream { +public: + const char *get_name() + { + return "VFR_HUD"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamVFRHUD(); + } + +private: + MavlinkOrbSubscription *att_sub; + struct vehicle_attitude_s *att; + + MavlinkOrbSubscription *pos_sub; + struct vehicle_global_position_s *pos; + + MavlinkOrbSubscription *armed_sub; + struct actuator_armed_s *armed; + + MavlinkOrbSubscription *act_sub; + struct actuator_controls_s *act; + + MavlinkOrbSubscription *airspeed_sub; + struct airspeed_s *airspeed; + +protected: + void subscribe(Mavlink *mavlink) + { + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); + att = (struct vehicle_attitude_s *)att_sub->get_data(); + + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s)); + pos = (struct vehicle_global_position_s *)pos_sub->get_data(); + + armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed), sizeof(struct actuator_armed_s)); + armed = (struct actuator_armed_s *)armed_sub->get_data(); + + act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0), sizeof(struct actuator_controls_s)); + act = (struct actuator_controls_s *)act_sub->get_data(); + + airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed), sizeof(struct airspeed_s)); + airspeed = (struct airspeed_s *)airspeed_sub->get_data(); + } + + void send(const hrt_abstime t) { + att_sub->update(t); + pos_sub->update(t); + armed_sub->update(t); + act_sub->update(t); + airspeed_sub->update(t); + + float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e); + uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F; + float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f; + + mavlink_msg_vfr_hud_send(_channel, + airspeed->true_airspeed_m_s, + groundspeed, + heading, + throttle, + pos->alt, + -pos->vel_d); + } +}; + + class MavlinkStreamGPSRawInt : public MavlinkStream { public: const char *get_name() @@ -638,11 +729,203 @@ protected: }; +class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream { +public: + const char *get_name() + { + return "GLOBAL_POSITION_SETPOINT_INT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamGlobalPositionSetpointInt(); + } + +private: + MavlinkOrbSubscription *pos_sp_triplet_sub; + struct position_setpoint_triplet_s *pos_sp_triplet; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); + pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); + } + + void send(const hrt_abstime t) { + pos_sp_triplet_sub->update(t); + + mavlink_msg_global_position_setpoint_int_send(_channel, + MAV_FRAME_GLOBAL, + (int32_t)(pos_sp_triplet->current.lat * 1e7), + (int32_t)(pos_sp_triplet->current.lon * 1e7), + (int32_t)(pos_sp_triplet->current.alt * 1000), + (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); + } +}; + + +class MavlinkStreamLocalPositionSetpoint : public MavlinkStream { +public: + const char *get_name() + { + return "LOCAL_POSITION_SETPOINT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamLocalPositionSetpoint(); + } + +private: + MavlinkOrbSubscription *pos_sp_sub; + struct vehicle_local_position_setpoint_s *pos_sp; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint), sizeof(vehicle_local_position_setpoint_s)); + pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data(); + } + + void send(const hrt_abstime t) { + pos_sp_sub->update(t); + + mavlink_msg_local_position_setpoint_send(_channel, + MAV_FRAME_LOCAL_NED, + pos_sp->x, + pos_sp->y, + pos_sp->z, + pos_sp->yaw); + } +}; + + +class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream { +public: + const char *get_name() + { + return "ROLL_PITCH_YAW_THRUST_SETPOINT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamRollPitchYawThrustSetpoint(); + } + +private: + MavlinkOrbSubscription *att_sp_sub; + struct vehicle_attitude_setpoint_s *att_sp; + +protected: + void subscribe(Mavlink *mavlink) + { + att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint), sizeof(vehicle_attitude_setpoint_s)); + att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); + } + + void send(const hrt_abstime t) { + att_sp_sub->update(t); + + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, + att_sp->timestamp / 1000, + att_sp->roll_body, + att_sp->pitch_body, + att_sp->yaw_body, + att_sp->thrust); + } +}; + + +class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream { +public: + const char *get_name() + { + return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); + } + +private: + MavlinkOrbSubscription *att_rates_sp_sub; + struct vehicle_rates_setpoint_s *att_rates_sp; + +protected: + void subscribe(Mavlink *mavlink) + { + att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint), sizeof(vehicle_rates_setpoint_s)); + att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data(); + } + + void send(const hrt_abstime t) { + att_rates_sp_sub->update(t); + + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, + att_rates_sp->timestamp / 1000, + att_rates_sp->roll, + att_rates_sp->pitch, + att_rates_sp->yaw, + att_rates_sp->thrust); + } +}; + + +class MavlinkStreamRCChannelsRaw : public MavlinkStream { +public: + const char *get_name() + { + return "RC_CHANNELS_RAW"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamRCChannelsRaw(); + } + +private: + MavlinkOrbSubscription *rc_sub; + struct rc_input_values *rc; + +protected: + void subscribe(Mavlink *mavlink) + { + rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc), sizeof(struct rc_input_values)); + rc = (struct rc_input_values *)rc_sub->get_data(); + } + + void send(const hrt_abstime t) { + rc_sub->update(t); + + const unsigned port_width = 8; + + for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { + /* Channels are sent in MAVLink main loop at a fixed interval */ + mavlink_msg_rc_channels_raw_send(_channel, + rc->timestamp_publication / 1000, + i, + (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, + rc->rssi); + } + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), new MavlinkStreamHighresIMU(), new MavlinkStreamAttitude(), + new MavlinkStreamVFRHUD(), new MavlinkStreamGPSRawInt(), new MavlinkStreamGlobalPositionInt(), new MavlinkStreamLocalPositionNED(), @@ -652,6 +935,11 @@ MavlinkStream *streams_list[] = { new MavlinkStreamServoOutputRaw(2), new MavlinkStreamServoOutputRaw(3), new MavlinkStreamHILControls(), + new MavlinkStreamGlobalPositionSetpointInt(), + new MavlinkStreamLocalPositionSetpoint(), + new MavlinkStreamRollPitchYawThrustSetpoint(), + new MavlinkStreamRollPitchYawRatesThrustSetpoint(), + new MavlinkStreamRCChannelsRaw(), nullptr }; @@ -705,132 +993,9 @@ MavlinkStream *streams_list[] = { // l->listener->attitude_counter++; //} // -//void -//MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l) -//{ -// struct vehicle_gps_position_s gps; -// -// /* copy gps data into local buffer */ -// orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_subs()->gps_sub, &gps); -// -// /* update SAT info every 10 seconds */ -// if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) { -// mavlink_msg_gps_status_send(l->mavlink->get_chan(), -// gps.satellites_visible, -// gps.satellite_prn, -// gps.satellite_used, -// gps.satellite_elevation, -// gps.satellite_azimuth, -// gps.satellite_snr); -// } // -// l->listener->gps_counter++; -//} // // -//void -//MavlinkOrbListener::l_input_rc(const struct listener *l) -//{ -// /* copy rc _mavlink->get_chan()nels into local buffer */ -// orb_copy(ORB_ID(input_rc), l->mavlink->get_subs()->input_rc_sub, &l->listener->rc_raw); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { -// -// const unsigned port_width = 8; -// -// for (unsigned i = 0; (i * port_width) < l->listener->rc_raw.channel_count; i++) { -// /* Channels are sent in MAVLink main loop at a fixed interval */ -// mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(), -// l->listener->rc_raw.timestamp_publication / 1000, -// i, -// (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 2) ? l->listener->rc_raw.values[(i * port_width) + 2] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 3) ? l->listener->rc_raw.values[(i * port_width) + 3] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 4) ? l->listener->rc_raw.values[(i * port_width) + 4] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 5) ? l->listener->rc_raw.values[(i * port_width) + 5] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 6) ? l->listener->rc_raw.values[(i * port_width) + 6] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 7) ? l->listener->rc_raw.values[(i * port_width) + 7] : UINT16_MAX, -// l->listener->rc_raw.rssi); -// } -// } -//} -// -// -//void -//MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) -//{ -// struct position_setpoint_triplet_s triplet; -// orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet); -// -// if (!triplet.current.valid) -// return; -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_global_position_setpoint_int_send(l->mavlink->get_chan(), -// MAV_FRAME_GLOBAL, -// (int32_t)(triplet.current.lat * 1e7d), -// (int32_t)(triplet.current.lon * 1e7d), -// (int32_t)(triplet.current.alt * 1e3f), -// (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); -//} -// -//void -//MavlinkOrbListener::l_local_position_setpoint(const struct listener *l) -//{ -// struct vehicle_local_position_setpoint_s local_sp; -// -// /* copy local position data into local buffer */ -// orb_copy(ORB_ID(vehicle_local_position_setpoint), l->mavlink->get_subs()->spl_sub, &local_sp); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_local_position_setpoint_send(l->mavlink->get_chan(), -// MAV_FRAME_LOCAL_NED, -// local_sp.x, -// local_sp.y, -// local_sp.z, -// local_sp.yaw); -//} -// -//void -//MavlinkOrbListener::l_attitude_setpoint(const struct listener *l) -//{ -// struct vehicle_attitude_setpoint_s att_sp; -// -// /* copy local position data into local buffer */ -// orb_copy(ORB_ID(vehicle_attitude_setpoint), l->mavlink->get_subs()->spa_sub, &att_sp); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(), -// att_sp.timestamp / 1000, -// att_sp.roll_body, -// att_sp.pitch_body, -// att_sp.yaw_body, -// att_sp.thrust); -//} -// -//void -//MavlinkOrbListener::l_vehicle_rates_setpoint(const struct listener *l) -//{ -// struct vehicle_rates_setpoint_s rates_sp; -// -// /* copy local position data into local buffer */ -// orb_copy(ORB_ID(vehicle_rates_setpoint), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(l->mavlink->get_chan(), -// rates_sp.timestamp / 1000, -// rates_sp.roll, -// rates_sp.pitch, -// rates_sp.yaw, -// rates_sp.thrust); -//} -// -//void -//MavlinkOrbListener::l_actuator_armed(const struct listener *l) -//{ -// orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); -//} // //void //MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l) -- cgit v1.2.3 From 77d1989abae9d267bb74f86fb0104dbefcbcd52a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 00:06:30 +0400 Subject: mavlink: more message streams implemented --- src/modules/mavlink/mavlink_messages.cpp | 144 +++++++++++++++++-------------- 1 file changed, 78 insertions(+), 66 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 3634acefa..8097ecdb3 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -335,6 +335,45 @@ protected: }; +class MavlinkStreamAttitudeQuaternion : public MavlinkStream { +public: + const char *get_name() + { + return "ATTITUDE_QUATERNION"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamAttitudeQuaternion(); + } + +private: + MavlinkOrbSubscription *att_sub; + struct vehicle_attitude_s *att; + +protected: + void subscribe(Mavlink *mavlink) + { + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); + att = (struct vehicle_attitude_s *)att_sub->get_data(); + } + + void send(const hrt_abstime t) { + att_sub->update(t); + + mavlink_msg_attitude_quaternion_send(_channel, + att->timestamp / 1000, + att->q[0], + att->q[1], + att->q[2], + att->q[3], + att->rollspeed, + att->pitchspeed, + att->yawspeed); + } +}; + + class MavlinkStreamVFRHUD : public MavlinkStream { public: const char *get_name() @@ -920,11 +959,49 @@ protected: }; +class MavlinkStreamManualControl : public MavlinkStream { +public: + const char *get_name() + { + return "MANUAL_CONTROL"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamManualControl(); + } + +private: + MavlinkOrbSubscription *manual_sub; + struct manual_control_setpoint_s *manual; + +protected: + void subscribe(Mavlink *mavlink) + { + manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint), sizeof(struct manual_control_setpoint_s)); + manual = (struct manual_control_setpoint_s *)manual_sub->get_data(); + } + + void send(const hrt_abstime t) { + manual_sub->update(t); + + mavlink_msg_manual_control_send(_channel, + mavlink_system.sysid, + manual->roll * 1000, + manual->pitch * 1000, + manual->yaw * 1000, + manual->throttle * 1000, + 0); + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), new MavlinkStreamHighresIMU(), new MavlinkStreamAttitude(), + new MavlinkStreamAttitudeQuaternion(), new MavlinkStreamVFRHUD(), new MavlinkStreamGPSRawInt(), new MavlinkStreamGlobalPositionInt(), @@ -940,6 +1017,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamRollPitchYawThrustSetpoint(), new MavlinkStreamRollPitchYawRatesThrustSetpoint(), new MavlinkStreamRCChannelsRaw(), + new MavlinkStreamManualControl(), nullptr }; @@ -949,73 +1027,13 @@ MavlinkStream *streams_list[] = { -//void -//MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) -//{ -// /* copy attitude data into local buffer */ -// orb_copy(ORB_ID(vehicle_attitude), l->mavlink->get_subs()->att_sub, &l->listener->att); // -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { -// /* send sensor values */ -// mavlink_msg_attitude_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// l->listener->att.roll, -// l->listener->att.pitch, -// l->listener->att.yaw, -// l->listener->att.rollspeed, -// l->listener->att.pitchspeed, -// l->listener->att.yawspeed); -// -// /* limit VFR message rate to 10Hz */ -// hrt_abstime t = hrt_absolute_time(); -// if (t >= l->listener->last_sent_vfr + 100000) { -// l->listener->last_sent_vfr = t; -// float groundspeed = sqrtf(l->listener->global_pos.vel_n * l->listener->global_pos.vel_n + l->listener->global_pos.vel_e * l->listener->global_pos.vel_e); -// uint16_t heading = _wrap_2pi(l->listener->att.yaw) * M_RAD_TO_DEG_F; -// float throttle = l->listener->armed.armed ? l->listener->actuators_0.control[3] * 100.0f : 0.0f; -// mavlink_msg_vfr_hud_send(l->mavlink->get_chan(), l->listener->airspeed.true_airspeed_m_s, groundspeed, heading, throttle, l->listener->global_pos.alt, -l->listener->global_pos.vel_d); -// } -// -// /* send quaternion values if it exists */ -// if(l->listener->att.q_valid) { -// mavlink_msg_attitude_quaternion_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// l->listener->att.q[0], -// l->listener->att.q[1], -// l->listener->att.q[2], -// l->listener->att.q[3], -// l->listener->att.rollspeed, -// l->listener->att.pitchspeed, -// l->listener->att.yawspeed); -// } -// } -// -// l->listener->attitude_counter++; -//} // // // // // //void -//MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l) -//{ -// struct manual_control_setpoint_s man_control; -// -// /* copy manual control data into local buffer */ -// orb_copy(ORB_ID(manual_control_setpoint), l->mavlink->get_subs()->man_control_sp_sub, &man_control); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_manual_control_send(l->mavlink->get_chan(), -// mavlink_system.sysid, -// man_control.roll * 1000, -// man_control.pitch * 1000, -// man_control.yaw * 1000, -// man_control.throttle * 1000, -// 0); -//} -// -//void //MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l) //{ // orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0); @@ -1069,12 +1087,6 @@ MavlinkStream *streams_list[] = { //} // //void -//MavlinkOrbListener::l_airspeed(const struct listener *l) -//{ -// orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed); -//} -// -//void //MavlinkOrbListener::l_nav_cap(const struct listener *l) //{ // -- cgit v1.2.3 From 836f7c435fe31572e45333877142dce8b4d2fc78 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 00:16:51 +0400 Subject: mavlink: code style and copyright fixes --- src/modules/mavlink/mavlink_main.cpp | 448 ++++++++++++-------- src/modules/mavlink/mavlink_main.h | 35 +- src/modules/mavlink/mavlink_messages.cpp | 495 +++++++++++++---------- src/modules/mavlink/mavlink_messages.h | 41 +- src/modules/mavlink/mavlink_orb_subscription.cpp | 44 +- src/modules/mavlink/mavlink_orb_subscription.h | 44 +- src/modules/mavlink/mavlink_rate_limiter.cpp | 44 +- src/modules/mavlink/mavlink_rate_limiter.h | 44 +- src/modules/mavlink/mavlink_receiver.cpp | 33 +- src/modules/mavlink/mavlink_receiver.h | 4 +- src/modules/mavlink/mavlink_stream.cpp | 42 +- src/modules/mavlink/mavlink_stream.h | 44 +- 12 files changed, 867 insertions(+), 451 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 672daf641..c97786553 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -37,6 +37,7 @@ * * @author Lorenz Meier * @author Julian Oes + * @author Anton Babushkin */ #include @@ -91,7 +92,7 @@ static const int ERROR = -1; #define MAX_DATA_RATE 10000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz -static Mavlink* _head = nullptr; +static Mavlink *_head = nullptr; /* TODO: if this is a class member it crashes */ static struct file_operations fops; @@ -112,40 +113,47 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length int uart = -1; switch (channel) { - case MAVLINK_COMM_0: - uart = Mavlink::get_uart_fd(0); + case MAVLINK_COMM_0: + uart = Mavlink::get_uart_fd(0); break; - case MAVLINK_COMM_1: - uart = Mavlink::get_uart_fd(1); + + case MAVLINK_COMM_1: + uart = Mavlink::get_uart_fd(1); break; - case MAVLINK_COMM_2: - uart = Mavlink::get_uart_fd(2); + + case MAVLINK_COMM_2: + uart = Mavlink::get_uart_fd(2); break; - case MAVLINK_COMM_3: - uart = Mavlink::get_uart_fd(3); + + case MAVLINK_COMM_3: + uart = Mavlink::get_uart_fd(3); break; - #ifdef MAVLINK_COMM_4 - case MAVLINK_COMM_4: - uart = Mavlink::get_uart_fd(4); +#ifdef MAVLINK_COMM_4 + + case MAVLINK_COMM_4: + uart = Mavlink::get_uart_fd(4); break; - #endif - #ifdef MAVLINK_COMM_5 - case MAVLINK_COMM_5: - uart = Mavlink::get_uart_fd(5); +#endif +#ifdef MAVLINK_COMM_5 + + case MAVLINK_COMM_5: + uart = Mavlink::get_uart_fd(5); break; - #endif - #ifdef MAVLINK_COMM_6 - case MAVLINK_COMM_6: - uart = Mavlink::get_uart_fd(6); +#endif +#ifdef MAVLINK_COMM_6 + + case MAVLINK_COMM_6: + uart = Mavlink::get_uart_fd(6); break; - #endif +#endif } ssize_t desired = (sizeof(uint8_t) * length); ssize_t ret = write(uart, ch, desired); - if (ret != desired) + if (ret != desired) { warn("write err"); + } } @@ -168,7 +176,7 @@ Mavlink::Mavlink() : _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { wpm = &wpm_s; - fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl; + fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; // _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); } @@ -206,8 +214,9 @@ int Mavlink::instance_count() { /* note: a local buffer count will help if this ever is called often */ - Mavlink* inst = ::_head; + Mavlink *inst = ::_head; unsigned inst_index = 0; + while (inst != nullptr) { inst = inst->next; inst_index++; @@ -225,14 +234,17 @@ Mavlink::new_instance() /* create the first instance at _head */ if (::_head == nullptr) { ::_head = inst; - /* afterwards follow the next and append the instance */ + /* afterwards follow the next and append the instance */ + } else { while (next->next != nullptr) { next = next->next; } + /* now parent has a null pointer, fill it */ next->next = inst; } + return inst; } @@ -241,6 +253,7 @@ Mavlink::get_instance(unsigned instance) { Mavlink *inst = ::_head; unsigned inst_index = 0; + while (inst->next != nullptr && inst_index < instance) { inst = inst->next; inst_index++; @@ -277,6 +290,7 @@ Mavlink::destroy_all_instances() unsigned iterations = 0; warnx("waiting for instances to stop"); + while (next_inst != nullptr) { inst_to_del = next_inst; @@ -284,6 +298,7 @@ Mavlink::destroy_all_instances() /* set flag to stop thread and wait for all threads to finish */ inst_to_del->_task_should_exit = true; + while (inst_to_del->thread_running) { printf("."); usleep(10000); @@ -294,6 +309,7 @@ Mavlink::destroy_all_instances() return ERROR; } } + delete inst_to_del; } @@ -308,23 +324,29 @@ Mavlink::destroy_all_instances() bool Mavlink::instance_exists(const char *device_name, Mavlink *self) { - Mavlink* inst = ::_head; + Mavlink *inst = ::_head; + while (inst != nullptr) { /* don't compare with itself */ - if (inst != self && !strcmp(device_name, inst->device_name)) + if (inst != self && !strcmp(device_name, inst->device_name)) { return true; + } + inst = inst->next; } + return false; } int Mavlink::get_uart_fd(unsigned index) { - Mavlink* inst = get_instance(index); - if (inst) + Mavlink *inst = get_instance(index); + + if (inst) { return inst->get_uart_fd(); + } return -1; } @@ -353,21 +375,23 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { - const char *txt = (const char *)arg; + const char *txt = (const char *)arg; // printf("logmsg: %s\n", txt); - struct mavlink_logmessage msg; - strncpy(msg.text, txt, sizeof(msg.text)); + struct mavlink_logmessage msg; + strncpy(msg.text, txt, sizeof(msg.text)); + + Mavlink *inst = ::_head; - Mavlink* inst = ::_head; - while (inst != nullptr) { + while (inst != nullptr) { - mavlink_logbuffer_write(&inst->lb, &msg); - inst->total_counter++; - inst = inst->next; + mavlink_logbuffer_write(&inst->lb, &msg); + inst->total_counter++; + inst = inst->next; + } + + return OK; } - return OK; - } default: return ENOTTY; @@ -582,7 +606,7 @@ int Mavlink::mavlink_pm_send_param_for_name(const char *name) int Mavlink::mavlink_pm_send_param(param_t param) { - if (param == PARAM_INVALID) return 1; + if (param == PARAM_INVALID) { return 1; } /* buffers for param transmission */ static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; @@ -714,38 +738,40 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item { /* only support global waypoints for now */ switch (mavlink_mission_item->frame) { - case MAV_FRAME_GLOBAL: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = false; - break; + case MAV_FRAME_GLOBAL: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = false; + break; - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = true; - break; + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = true; + break; - case MAV_FRAME_LOCAL_NED: - case MAV_FRAME_LOCAL_ENU: - return MAV_MISSION_UNSUPPORTED_FRAME; - case MAV_FRAME_MISSION: - default: - return MAV_MISSION_ERROR; + case MAV_FRAME_LOCAL_NED: + case MAV_FRAME_LOCAL_ENU: + return MAV_MISSION_UNSUPPORTED_FRAME; + + case MAV_FRAME_MISSION: + default: + return MAV_MISSION_ERROR; } switch (mavlink_mission_item->command) { - case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param2; - break; - default: - mission_item->acceptance_radius = mavlink_mission_item->param2; - break; + case MAV_CMD_NAV_TAKEOFF: + mission_item->pitch_min = mavlink_mission_item->param2; + break; + + default: + mission_item->acceptance_radius = mavlink_mission_item->param2; + break; } - mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F); + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; @@ -762,25 +788,27 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ { if (mission_item->altitude_is_relative) { mavlink_mission_item->frame = MAV_FRAME_GLOBAL; + } else { mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; } - + switch (mission_item->nav_cmd) { - case NAV_CMD_TAKEOFF: - mavlink_mission_item->param2 = mission_item->pitch_min; - break; - default: - mavlink_mission_item->param2 = mission_item->acceptance_radius; - break; + case NAV_CMD_TAKEOFF: + mavlink_mission_item->param2 = mission_item->pitch_min; + break; + + default: + mavlink_mission_item->param2 = mission_item->acceptance_radius; + break; } mavlink_mission_item->x = (float)mission_item->lat; mavlink_mission_item->y = (float)mission_item->lon; mavlink_mission_item->z = mission_item->altitude; - mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F; - mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction; + mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; + mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; mavlink_mission_item->command = mission_item->nav_cmd; mavlink_mission_item->param1 = mission_item->time_inside; mavlink_mission_item->autocontinue = mission_item->autocontinue; @@ -817,7 +845,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8 mavlink_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa); mavlink_missionlib_send_message(&msg); - if (_verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); + if (_verbose) { warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); } } /* @@ -846,7 +874,8 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) } else { mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); - if (_verbose) warnx("ERROR: index out of bounds"); + + if (_verbose) { warnx("ERROR: index out of bounds"); } } } @@ -862,7 +891,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin mavlink_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); - if (_verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); + if (_verbose) { warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); } } void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) @@ -870,11 +899,12 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t struct mission_item_s mission_item; ssize_t len = sizeof(struct mission_item_s); - + dm_item_t dm_current; if (wpm->current_dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } @@ -892,10 +922,12 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t mavlink_msg_mission_item_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp); mavlink_missionlib_send_message(&msg); - if (_verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); + if (_verbose) { warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); } + } else { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - if (_verbose) warnx("ERROR: could not read WP%u", seq); + + if (_verbose) { warnx("ERROR: could not read WP%u", seq); } } } @@ -910,11 +942,12 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u mavlink_msg_mission_request_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpr); mavlink_missionlib_send_message(&msg); - if (_verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); + if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); } } else { mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); - if (_verbose) warnx("ERROR: Waypoint index exceeds list capacity"); + + if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); } } } @@ -935,7 +968,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp_reached); mavlink_missionlib_send_message(&msg); - if (_verbose) warnx("Sent waypoint %u reached message", wp_reached.seq); + if (_verbose) { warnx("Sent waypoint %u reached message", wp_reached.seq); } } void Mavlink::mavlink_waypoint_eventloop(uint64_t now) @@ -945,7 +978,7 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) mavlink_missionlib_send_gcs_string("Operation timeout"); - if (_verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state); + if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state); } wpm->current_state = MAVLINK_WPM_STATE_IDLE; wpm->current_partner_sysid = 0; @@ -960,7 +993,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) switch (msg->msgid) { - case MAVLINK_MSG_ID_MISSION_ACK: { + case MAVLINK_MSG_ID_MISSION_ACK: { mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(msg, &wpa); @@ -977,13 +1010,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); - if (_verbose) warnx("REJ. WP CMD: curr partner id mismatch"); + + if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); } } break; } - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { mavlink_mission_set_current_t wpc; mavlink_msg_mission_set_current_decode(msg, &wpc); @@ -995,30 +1029,33 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mission.current_index = wpc.seq; publish_mission(); - + /* don't answer yet, wait for the navigator to respond, then publish the mission_result */ // mavlink_wpm_send_waypoint_current(wpc.seq); } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); - if (_verbose) warnx("IGN WP CURR CMD: Not in list"); + + if (_verbose) { warnx("IGN WP CURR CMD: Not in list"); } } } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - if (_verbose) warnx("IGN WP CURR CMD: Busy"); + + 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"); + + if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); } } break; } - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { mavlink_mission_request_list_t wprl; mavlink_msg_mission_request_list_decode(msg, &wprl); @@ -1027,14 +1064,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (wpm->size > 0) { - + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; wpm->current_wp_id = 0; wpm->current_partner_sysid = msg->sysid; wpm->current_partner_compid = msg->compid; } else { - if (_verbose) warnx("No waypoints send"); + if (_verbose) { warnx("No waypoints send"); } } wpm->current_count = wpm->size; @@ -1042,17 +1079,20 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); - if (_verbose) warnx("IGN REQUEST LIST: Busy"); + + 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"); + + if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); } } break; } - case MAVLINK_MSG_ID_MISSION_REQUEST: { + case MAVLINK_MSG_ID_MISSION_REQUEST: { mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(msg, &wpr); @@ -1062,22 +1102,28 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpr.seq >= wpm->size) { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); + + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); } + break; } - /* - * Ensure that we are in the correct state and that the first request has id 0 + /* + * Ensure that we are in the correct state and that the first request has id 0 * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) */ if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (wpr.seq == 0) { - if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); + if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); - if (_verbose) warnx("REJ. WP CMD: First id != 0"); + + if (_verbose) { warnx("REJ. WP CMD: First id != 0"); } + break; } @@ -1085,22 +1131,26 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpr.seq == wpm->current_wp_id) { - if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); + if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } } else if (wpr.seq == wpm->current_wp_id + 1) { - if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); - + if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); + + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); } + break; } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); + + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); } + break; } @@ -1109,11 +1159,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpr.seq < wpm->size) { - mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid,wpm->current_wp_id); + mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); } else { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - if (_verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq); + + if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); } } @@ -1122,18 +1173,21 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) { 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); + + 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"); + + if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } + break; } - case MAVLINK_MSG_ID_MISSION_COUNT: { + case MAVLINK_MSG_ID_MISSION_COUNT: { mavlink_mission_count_t wpc; mavlink_msg_mission_count_decode(msg, &wpc); @@ -1143,18 +1197,21 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { if (wpc.count > NUM_MISSIONS_SUPPORTED) { - if (_verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); + if (_verbose) { warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); } + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_NO_SPACE); break; } if (wpc.count == 0) { mavlink_missionlib_send_gcs_string("COUNT 0"); - if (_verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); + + if (_verbose) { warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); } + break; } - - if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); + + if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } wpm->current_state = MAVLINK_WPM_STATE_GETLIST; wpm->current_wp_id = 0; @@ -1168,24 +1225,31 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpm->current_wp_id == 0) { mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); - if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); + + if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); } + } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id); + + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id); } } + } else { - mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); - if (_verbose) warnx("IGN MISSION_COUNT CMD: Busy"); + mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); + + 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"); + + if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } break; - case MAVLINK_MSG_ID_MISSION_ITEM: { + case MAVLINK_MSG_ID_MISSION_ITEM: { mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); @@ -1200,11 +1264,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - if (wp.seq != 0) { - mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq); - break; - } + if (wp.seq != 0) { + mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); + warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq); + break; + } + } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { if (wp.seq >= wpm->current_count) { @@ -1239,6 +1304,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpm->current_dataman_id == 0) { dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; mission.dataman_id = 1; + } else { dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0; mission.dataman_id = 0; @@ -1260,13 +1326,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) wpm->current_wp_id = wp.seq + 1; if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - - if (_verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count); + + if (_verbose) { warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count); } mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); mission.count = wpm->current_count; - + publish_mission(); wpm->current_dataman_id = mission.dataman_id; @@ -1280,13 +1346,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } 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"); + + if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; } - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { mavlink_mission_clear_all_t wpca; mavlink_msg_mission_clear_all_decode(msg, &wpca); @@ -1305,21 +1372,24 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + } else { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); } - + } else { mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); - if (_verbose) warnx("IGN WP CLEAR CMD: Busy"); + + 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"); + + if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1352,8 +1422,9 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) while (i < len - 1) { statustext.text[i] = string[i]; - if (string[i] == '\0') + if (string[i] == '\0') { break; + } i++; } @@ -1411,6 +1482,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate) LL_DELETE(_streams, stream); delete stream; } + return OK; } } @@ -1466,18 +1538,22 @@ Mavlink::task_main(int argc, char *argv[]) switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); + if (_baudrate < 9600 || _baudrate > 921600) { warnx("invalid baud rate '%s'", optarg); err_flag = true; } + break; case 'r': _datarate = strtoul(optarg, NULL, 10); + if (_datarate < 10 || _datarate > MAX_DATA_RATE) { warnx("invalid data rate '%s'", optarg); err_flag = true; } + break; case 'd': @@ -1501,6 +1577,7 @@ Mavlink::task_main(int argc, char *argv[]) } else if (strcmp(optarg, "custom") == 0) { _mode = MODE_CUSTOM; } + break; case 'v': @@ -1533,35 +1610,41 @@ Mavlink::task_main(int argc, char *argv[]) /* inform about mode */ switch (_mode) { - case MODE_CUSTOM: - warnx("mode: CUSTOM"); - break; - case MODE_OFFBOARD: - warnx("mode: OFFBOARD"); - break; - case MODE_ONBOARD: - warnx("mode: ONBOARD"); - break; - case MODE_HIL: - warnx("mode: HIL"); - break; - default: - warnx("ERROR: Unknown mode"); - break; + case MODE_CUSTOM: + warnx("mode: CUSTOM"); + break; + + case MODE_OFFBOARD: + warnx("mode: OFFBOARD"); + break; + + case MODE_ONBOARD: + warnx("mode: ONBOARD"); + break; + + case MODE_HIL: + warnx("mode: HIL"); + break; + + default: + warnx("ERROR: Unknown mode"); + break; } - switch(_mode) { - case MODE_OFFBOARD: - case MODE_HIL: - case MODE_CUSTOM: - _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; - break; - case MODE_ONBOARD: - _mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA; - break; - default: - _mavlink_wpm_comp_id = MAV_COMP_ID_ALL; - break; + switch (_mode) { + case MODE_OFFBOARD: + case MODE_HIL: + case MODE_CUSTOM: + _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; + break; + + case MODE_ONBOARD: + _mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA; + break; + + default: + _mavlink_wpm_comp_id = MAV_COMP_ID_ALL; + break; } warnx("data rate: %d bytes/s", _datarate); @@ -1576,8 +1659,9 @@ Mavlink::task_main(int argc, char *argv[]) /* default values for arguments */ _uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart); - if (_uart < 0) + if (_uart < 0) { err(1, "could not open %s", device_name); + } /* create the device node that's used for sending text log messages, etc. */ if (instance_count() == 1) { @@ -1615,7 +1699,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("HEARTBEAT", 1.0f); - switch(_mode) { + switch (_mode) { case MODE_OFFBOARD: configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); @@ -1660,10 +1744,12 @@ Mavlink::task_main(int argc, char *argv[]) if (status_sub->update(t)) { /* switch HIL mode if required */ - if (status->hil_state == HIL_STATE_ON) + if (status->hil_state == HIL_STATE_ON) { set_hil_enabled(true); - else if (status->hil_state == HIL_STATE_OFF) + + } else if (status->hil_state == HIL_STATE_OFF) { set_hil_enabled(false); + } } MavlinkStream *stream; @@ -1677,13 +1763,14 @@ Mavlink::task_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); - if (_verbose) warnx("Got mission result: new current: %d", mission_result.index_current_mission); + if (_verbose) { warnx("Got mission result: new current: %d", mission_result.index_current_mission); } if (mission_result.mission_reached) { mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached); } mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); + } else { if (slow_rate_limiter.check(t)) { mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); @@ -1740,11 +1827,11 @@ Mavlink::start(int argc, char *argv[]) sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); /*mavlink->_mavlink_task = */task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); // while (!this->is_running()) { // usleep(200); @@ -1775,7 +1862,7 @@ Mavlink::start(int argc, char *argv[]) } void -Mavlink::status() +Mavlink::status() { warnx("running"); } @@ -1799,9 +1886,11 @@ Mavlink::stream(int argc, char *argv[]) switch (ch) { case 'r': rate = strtod(optarg, nullptr); + if (rate < 0.0f) { err_flag = true; } + break; case 'd': @@ -1820,6 +1909,7 @@ Mavlink::stream(int argc, char *argv[]) if (!err_flag && rate >= 0.0 && stream_name != nullptr) { Mavlink *inst = get_instance_for_device(device_name); + if (inst != nullptr) { if (OK == inst->configure_stream(stream_name, rate)) { if (rate > 0.0f) { @@ -1865,15 +1955,15 @@ int mavlink_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop-all")) { return Mavlink::destroy_all_instances(); - // } else if (!strcmp(argv[1], "status")) { - // mavlink::g_mavlink->status(); + // } else if (!strcmp(argv[1], "status")) { + // mavlink::g_mavlink->status(); } else if (!strcmp(argv[1], "stream")) { return Mavlink::stream(argc, argv); - } else { - usage(); - } + } else { + usage(); + } return 0; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 41e781ee8..532c9bcee 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -36,6 +36,7 @@ * MAVLink 1.0 protocol interface definition. * * @author Lorenz Meier + * @author Anton Babushkin */ #pragma once @@ -55,24 +56,24 @@ #include "mavlink_stream.h" #include "mavlink_messages.h" - // FIXME XXX - TO BE MOVED TO XML +// FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_SENDLIST_SENDWPS, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_GETLIST_GETWPS, + MAVLINK_WPM_STATE_GETLIST_GOTALL, + MAVLINK_WPM_STATE_ENUM_END }; enum MAVLINK_WPM_CODES { - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END }; @@ -144,7 +145,7 @@ public: const char *device_name; enum MAVLINK_MODE { - MODE_CUSTOM=0, + MODE_CUSTOM = 0, MODE_OFFBOARD, MODE_ONBOARD, MODE_HIL @@ -186,7 +187,7 @@ public: bool _task_should_exit; /**< if true, mavlink task should exit */ protected: - Mavlink* next; + Mavlink *next; private: int _mavlink_fd; @@ -233,7 +234,7 @@ private: unsigned int mavlink_param_queue_index; bool mavlink_link_termination_allowed; - + /** * Send one parameter. * diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 8097ecdb3..820faae1c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1,8 +1,42 @@ -/* - * mavlink_messages.cpp +/**************************************************************************** * - * Created on: 25.02.2014 - * Author: ton + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_messages.cpp + * MAVLink 1.0 message formatters implementation. + * + * @author Lorenz Meier + * @author Anton Babushkin */ #include @@ -43,7 +77,7 @@ static uint16_t cm_uint16_from_m_float(float m); static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, - uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); uint16_t cm_uint16_from_m_float(float m) @@ -59,7 +93,7 @@ cm_uint16_from_m_float(float m) } void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, - uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { *mavlink_state = 0; *mavlink_base_mode = 0; @@ -72,7 +106,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* arming state */ if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -81,34 +115,44 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; + if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { - /* use main state when navigator is not active */ + /* use main state when navigator is not active */ if (status->main_state == MAIN_STATE_MANUAL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (status->main_state == MAIN_STATE_SEATBELT) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + } else if (status->main_state == MAIN_STATE_EASY) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } + } else { /* use navigation state when navigator is active */ *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + if (pos_sp_triplet->nav_state == NAV_STATE_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; } @@ -118,24 +162,30 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* set system state */ if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; + } else if (status->arming_state == ARMING_STATE_ARMED) { *mavlink_state = MAV_STATE_ACTIVE; + } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_state = MAV_STATE_CRITICAL; + } else if (status->arming_state == ARMING_STATE_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; + } else if (status->arming_state == ARMING_STATE_REBOOT) { *mavlink_state = MAV_STATE_POWEROFF; + } else { *mavlink_state = MAV_STATE_CRITICAL; } } -class MavlinkStreamHeartbeat : public MavlinkStream { +class MavlinkStreamHeartbeat : public MavlinkStream +{ public: const char *get_name() { @@ -164,7 +214,8 @@ protected: pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { status_sub->update(t); pos_sp_triplet_sub->update(t); @@ -184,7 +235,8 @@ protected: }; -class MavlinkStreamSysStatus : public MavlinkStream { +class MavlinkStreamSysStatus : public MavlinkStream +{ public: const char *get_name() { @@ -207,28 +259,30 @@ protected: status = (struct vehicle_status_s *)status_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { status_sub->update(t); mavlink_msg_sys_status_send(_channel, - status->onboard_control_sensors_present, - status->onboard_control_sensors_enabled, - status->onboard_control_sensors_health, - status->load * 1000.0f, - status->battery_voltage * 1000.0f, - status->battery_current * 1000.0f, - status->battery_remaining, - status->drop_rate_comm, - status->errors_comm, - status->errors_count1, - status->errors_count2, - status->errors_count3, - status->errors_count4); + status->onboard_control_sensors_present, + status->onboard_control_sensors_enabled, + status->onboard_control_sensors_health, + status->load * 1000.0f, + status->battery_voltage * 1000.0f, + status->battery_current * 1000.0f, + status->battery_remaining, + status->drop_rate_comm, + status->errors_comm, + status->errors_count1, + status->errors_count2, + status->errors_count3, + status->errors_count4); } }; -class MavlinkStreamHighresIMU : public MavlinkStream { +class MavlinkStreamHighresIMU : public MavlinkStream +{ public: MavlinkStreamHighresIMU() : MavlinkStream(), accel_counter(0), gyro_counter(0), mag_counter(0), baro_counter(0) { @@ -260,7 +314,8 @@ protected: sensor = (struct sensor_combined_s *)sensor_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { sensor_sub->update(t); uint16_t fields_updated = 0; @@ -290,18 +345,19 @@ protected: } mavlink_msg_highres_imu_send(_channel, - sensor->timestamp, - sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], - sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], - sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], - sensor->baro_pres_mbar, sensor->differential_pressure_pa, - sensor->baro_alt_meter, sensor->baro_temp_celcius, - fields_updated); + sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); } }; -class MavlinkStreamAttitude : public MavlinkStream { +class MavlinkStreamAttitude : public MavlinkStream +{ public: const char *get_name() { @@ -324,18 +380,20 @@ protected: att = (struct vehicle_attitude_s *)att_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { att_sub->update(t); mavlink_msg_attitude_send(_channel, - att->timestamp / 1000, - att->roll, att->pitch, att->yaw, - att->rollspeed, att->pitchspeed, att->yawspeed); + att->timestamp / 1000, + att->roll, att->pitch, att->yaw, + att->rollspeed, att->pitchspeed, att->yawspeed); } }; -class MavlinkStreamAttitudeQuaternion : public MavlinkStream { +class MavlinkStreamAttitudeQuaternion : public MavlinkStream +{ public: const char *get_name() { @@ -358,23 +416,25 @@ protected: att = (struct vehicle_attitude_s *)att_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { att_sub->update(t); mavlink_msg_attitude_quaternion_send(_channel, - att->timestamp / 1000, - att->q[0], - att->q[1], - att->q[2], - att->q[3], - att->rollspeed, - att->pitchspeed, - att->yawspeed); + att->timestamp / 1000, + att->q[0], + att->q[1], + att->q[2], + att->q[3], + att->rollspeed, + att->pitchspeed, + att->yawspeed); } }; -class MavlinkStreamVFRHUD : public MavlinkStream { +class MavlinkStreamVFRHUD : public MavlinkStream +{ public: const char *get_name() { @@ -421,7 +481,8 @@ protected: airspeed = (struct airspeed_s *)airspeed_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { att_sub->update(t); pos_sub->update(t); armed_sub->update(t); @@ -433,17 +494,18 @@ protected: float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f; mavlink_msg_vfr_hud_send(_channel, - airspeed->true_airspeed_m_s, - groundspeed, - heading, - throttle, - pos->alt, - -pos->vel_d); + airspeed->true_airspeed_m_s, + groundspeed, + heading, + throttle, + pos->alt, + -pos->vel_d); } }; -class MavlinkStreamGPSRawInt : public MavlinkStream { +class MavlinkStreamGPSRawInt : public MavlinkStream +{ public: const char *get_name() { @@ -466,25 +528,27 @@ protected: gps = (struct vehicle_gps_position_s *)gps_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { gps_sub->update(t); mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph_m), - cm_uint16_from_m_float(gps->epv_m), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); + gps->timestamp_position, + gps->fix_type, + gps->lat, + gps->lon, + gps->alt, + cm_uint16_from_m_float(gps->eph_m), + cm_uint16_from_m_float(gps->epv_m), + gps->vel_m_s * 100.0f, + _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps->satellites_visible); } }; -class MavlinkStreamGlobalPositionInt : public MavlinkStream { +class MavlinkStreamGlobalPositionInt : public MavlinkStream +{ public: const char *get_name() { @@ -513,25 +577,27 @@ protected: home = (struct home_position_s *)home_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { pos_sub->update(t); home_sub->update(t); mavlink_msg_global_position_int_send(_channel, - pos->timestamp / 1000, - pos->lat * 1e7, - pos->lon * 1e7, - pos->alt * 1000.0f, - (pos->alt - home->alt) * 1000.0f, - pos->vel_n * 100.0f, - pos->vel_e * 100.0f, - pos->vel_d * 100.0f, - _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + pos->timestamp / 1000, + pos->lat * 1e7, + pos->lon * 1e7, + pos->alt * 1000.0f, + (pos->alt - home->alt) * 1000.0f, + pos->vel_n * 100.0f, + pos->vel_e * 100.0f, + pos->vel_d * 100.0f, + _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); } }; -class MavlinkStreamLocalPositionNED : public MavlinkStream { +class MavlinkStreamLocalPositionNED : public MavlinkStream +{ public: const char *get_name() { @@ -554,22 +620,24 @@ protected: pos = (struct vehicle_local_position_s *)pos_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { pos_sub->update(t); mavlink_msg_local_position_ned_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->vx, - pos->vy, - pos->vz); + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->vx, + pos->vy, + pos->vz); } }; -class MavlinkStreamGPSGlobalOrigin : public MavlinkStream { +class MavlinkStreamGPSGlobalOrigin : public MavlinkStream +{ public: const char *get_name() { @@ -592,18 +660,20 @@ protected: home = (struct home_position_s *)home_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { home_sub->update(t); mavlink_msg_gps_global_origin_send(_channel, - (int32_t)(home->lat * 1e7), - (int32_t)(home->lon * 1e7), - (int32_t)(home->alt) * 1000.0f); + (int32_t)(home->lat * 1e7), + (int32_t)(home->lon * 1e7), + (int32_t)(home->alt) * 1000.0f); } }; -class MavlinkStreamServoOutputRaw : public MavlinkStream { +class MavlinkStreamServoOutputRaw : public MavlinkStream +{ public: MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n) { @@ -641,25 +711,27 @@ protected: act = (struct actuator_outputs_s *)act_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { act_sub->update(t); mavlink_msg_servo_output_raw_send(_channel, - act->timestamp / 1000, - _n, - act->output[0], - act->output[1], - act->output[2], - act->output[3], - act->output[4], - act->output[5], - act->output[6], - act->output[7]); + act->timestamp / 1000, + _n, + act->output[0], + act->output[1], + act->output[2], + act->output[3], + act->output[4], + act->output[5], + act->output[6], + act->output[7]); } }; -class MavlinkStreamHILControls : public MavlinkStream { +class MavlinkStreamHILControls : public MavlinkStream +{ public: const char *get_name() { @@ -694,7 +766,8 @@ protected: act = (struct actuator_outputs_s *)act_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { status_sub->update(t); pos_sp_triplet_sub->update(t); act_sub->update(t); @@ -710,65 +783,66 @@ protected: /* scale / assign outputs depending on system type */ if (mavlink_system.type == MAV_TYPE_QUADROTOR) { mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - -1, - -1, - mavlink_base_mode, - 0); + hrt_absolute_time(), + ((act->output[0] - 900.0f) / 600.0f) / 2.0f, + ((act->output[1] - 900.0f) / 600.0f) / 2.0f, + ((act->output[2] - 900.0f) / 600.0f) / 2.0f, + ((act->output[3] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + -1, + -1, + mavlink_base_mode, + 0); } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - ((act->output[4] - 900.0f) / 600.0f) / 2.0f, - ((act->output[5] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - mavlink_base_mode, - 0); + hrt_absolute_time(), + ((act->output[0] - 900.0f) / 600.0f) / 2.0f, + ((act->output[1] - 900.0f) / 600.0f) / 2.0f, + ((act->output[2] - 900.0f) / 600.0f) / 2.0f, + ((act->output[3] - 900.0f) / 600.0f) / 2.0f, + ((act->output[4] - 900.0f) / 600.0f) / 2.0f, + ((act->output[5] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + mavlink_base_mode, + 0); } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - ((act->output[4] - 900.0f) / 600.0f) / 2.0f, - ((act->output[5] - 900.0f) / 600.0f) / 2.0f, - ((act->output[6] - 900.0f) / 600.0f) / 2.0f, - ((act->output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_base_mode, - 0); + hrt_absolute_time(), + ((act->output[0] - 900.0f) / 600.0f) / 2.0f, + ((act->output[1] - 900.0f) / 600.0f) / 2.0f, + ((act->output[2] - 900.0f) / 600.0f) / 2.0f, + ((act->output[3] - 900.0f) / 600.0f) / 2.0f, + ((act->output[4] - 900.0f) / 600.0f) / 2.0f, + ((act->output[5] - 900.0f) / 600.0f) / 2.0f, + ((act->output[6] - 900.0f) / 600.0f) / 2.0f, + ((act->output[7] - 900.0f) / 600.0f) / 2.0f, + mavlink_base_mode, + 0); } else { mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - (act->output[0] - 1500.0f) / 500.0f, - (act->output[1] - 1500.0f) / 500.0f, - (act->output[2] - 1500.0f) / 500.0f, - (act->output[3] - 1000.0f) / 1000.0f, - (act->output[4] - 1500.0f) / 500.0f, - (act->output[5] - 1500.0f) / 500.0f, - (act->output[6] - 1500.0f) / 500.0f, - (act->output[7] - 1500.0f) / 500.0f, - mavlink_base_mode, - 0); + hrt_absolute_time(), + (act->output[0] - 1500.0f) / 500.0f, + (act->output[1] - 1500.0f) / 500.0f, + (act->output[2] - 1500.0f) / 500.0f, + (act->output[3] - 1000.0f) / 1000.0f, + (act->output[4] - 1500.0f) / 500.0f, + (act->output[5] - 1500.0f) / 500.0f, + (act->output[6] - 1500.0f) / 500.0f, + (act->output[7] - 1500.0f) / 500.0f, + mavlink_base_mode, + 0); } } }; -class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream { +class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +{ public: const char *get_name() { @@ -791,7 +865,8 @@ protected: pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { pos_sp_triplet_sub->update(t); mavlink_msg_global_position_setpoint_int_send(_channel, @@ -804,7 +879,8 @@ protected: }; -class MavlinkStreamLocalPositionSetpoint : public MavlinkStream { +class MavlinkStreamLocalPositionSetpoint : public MavlinkStream +{ public: const char *get_name() { @@ -827,7 +903,8 @@ protected: pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { pos_sp_sub->update(t); mavlink_msg_local_position_setpoint_send(_channel, @@ -840,7 +917,8 @@ protected: }; -class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream { +class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +{ public: const char *get_name() { @@ -863,7 +941,8 @@ protected: att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { att_sp_sub->update(t); mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, @@ -876,7 +955,8 @@ protected: }; -class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream { +class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream +{ public: const char *get_name() { @@ -899,7 +979,8 @@ protected: att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { att_rates_sp_sub->update(t); mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, @@ -912,7 +993,8 @@ protected: }; -class MavlinkStreamRCChannelsRaw : public MavlinkStream { +class MavlinkStreamRCChannelsRaw : public MavlinkStream +{ public: const char *get_name() { @@ -935,7 +1017,8 @@ protected: rc = (struct rc_input_values *)rc_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { rc_sub->update(t); const unsigned port_width = 8; @@ -943,23 +1026,24 @@ protected: for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { /* Channels are sent in MAVLink main loop at a fixed interval */ mavlink_msg_rc_channels_raw_send(_channel, - rc->timestamp_publication / 1000, - i, - (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, - rc->rssi); + rc->timestamp_publication / 1000, + i, + (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, + rc->rssi); } } }; -class MavlinkStreamManualControl : public MavlinkStream { +class MavlinkStreamManualControl : public MavlinkStream +{ public: const char *get_name() { @@ -982,43 +1066,44 @@ protected: manual = (struct manual_control_setpoint_s *)manual_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { manual_sub->update(t); mavlink_msg_manual_control_send(_channel, - mavlink_system.sysid, - manual->roll * 1000, - manual->pitch * 1000, - manual->yaw * 1000, - manual->throttle * 1000, - 0); + mavlink_system.sysid, + manual->roll * 1000, + manual->pitch * 1000, + manual->yaw * 1000, + manual->throttle * 1000, + 0); } }; MavlinkStream *streams_list[] = { - new MavlinkStreamHeartbeat(), - new MavlinkStreamSysStatus(), - new MavlinkStreamHighresIMU(), - new MavlinkStreamAttitude(), - new MavlinkStreamAttitudeQuaternion(), - new MavlinkStreamVFRHUD(), - new MavlinkStreamGPSRawInt(), - new MavlinkStreamGlobalPositionInt(), - new MavlinkStreamLocalPositionNED(), - new MavlinkStreamGPSGlobalOrigin(), - new MavlinkStreamServoOutputRaw(0), - new MavlinkStreamServoOutputRaw(1), - new MavlinkStreamServoOutputRaw(2), - new MavlinkStreamServoOutputRaw(3), - new MavlinkStreamHILControls(), - new MavlinkStreamGlobalPositionSetpointInt(), - new MavlinkStreamLocalPositionSetpoint(), - new MavlinkStreamRollPitchYawThrustSetpoint(), - new MavlinkStreamRollPitchYawRatesThrustSetpoint(), - new MavlinkStreamRCChannelsRaw(), - new MavlinkStreamManualControl(), - nullptr + new MavlinkStreamHeartbeat(), + new MavlinkStreamSysStatus(), + new MavlinkStreamHighresIMU(), + new MavlinkStreamAttitude(), + new MavlinkStreamAttitudeQuaternion(), + new MavlinkStreamVFRHUD(), + new MavlinkStreamGPSRawInt(), + new MavlinkStreamGlobalPositionInt(), + new MavlinkStreamLocalPositionNED(), + new MavlinkStreamGPSGlobalOrigin(), + new MavlinkStreamServoOutputRaw(0), + new MavlinkStreamServoOutputRaw(1), + new MavlinkStreamServoOutputRaw(2), + new MavlinkStreamServoOutputRaw(3), + new MavlinkStreamHILControls(), + new MavlinkStreamGlobalPositionSetpointInt(), + new MavlinkStreamLocalPositionSetpoint(), + new MavlinkStreamRollPitchYawThrustSetpoint(), + new MavlinkStreamRollPitchYawRatesThrustSetpoint(), + new MavlinkStreamRCChannelsRaw(), + new MavlinkStreamManualControl(), + nullptr }; diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index 3dc6cb699..b8823263a 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -1,8 +1,41 @@ -/* - * mavlink_messages.h +/**************************************************************************** * - * Created on: 25.02.2014 - * Author: ton + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_messages.h + * MAVLink 1.0 message formatters definition. + * + * @author Anton Babushkin */ #ifndef MAVLINK_MESSAGES_H_ diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 35470a19a..e1208bca9 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -1,10 +1,42 @@ -/* - * mavlink_orb_subscription.cpp +/**************************************************************************** * - * Created on: 23.02.2014 - * Author: ton - */ + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file mavlink_orb_subscription.cpp + * uORB subscription implementation. + * + * @author Anton Babushkin + */ #include #include @@ -45,10 +77,12 @@ MavlinkOrbSubscription::update(const hrt_abstime t) _last_check = t; bool updated; orb_check(_fd, &updated); + if (updated) { orb_copy(_topic, _fd, _data); return true; } } + return false; } diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 79ff3abdb..3cf33ccef 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -1,8 +1,41 @@ -/* - * mavlink_orb_subscription.h +/**************************************************************************** * - * Created on: 23.02.2014 - * Author: ton + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_orb_subscription.h + * uORB subscription definition. + * + * @author Anton Babushkin */ #ifndef MAVLINK_ORB_SUBSCRIPTION_H_ @@ -12,7 +45,8 @@ #include -class MavlinkOrbSubscription { +class MavlinkOrbSubscription +{ public: MavlinkOrbSubscription *next; diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp index f5bb06ccd..f6ed6e662 100644 --- a/src/modules/mavlink/mavlink_rate_limiter.cpp +++ b/src/modules/mavlink/mavlink_rate_limiter.cpp @@ -1,10 +1,42 @@ -/* - * mavlink_rate_limiter.cpp +/**************************************************************************** * - * Created on: 26.02.2014 - * Author: ton - */ + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file mavlink_rate_limiter.cpp + * Message rate limiter implementation. + * + * @author Anton Babushkin + */ #include "mavlink_rate_limiter.h" @@ -30,9 +62,11 @@ bool MavlinkRateLimiter::check(hrt_abstime t) { uint64_t dt = t - _last_sent; + if (dt > 0 && dt >= _interval) { _last_sent = (t / _interval) * _interval; return true; } + return false; } diff --git a/src/modules/mavlink/mavlink_rate_limiter.h b/src/modules/mavlink/mavlink_rate_limiter.h index 6db65f638..0b37538e6 100644 --- a/src/modules/mavlink/mavlink_rate_limiter.h +++ b/src/modules/mavlink/mavlink_rate_limiter.h @@ -1,8 +1,41 @@ -/* - * mavlink_rate_limiter.h +/**************************************************************************** * - * Created on: 26.02.2014 - * Author: ton + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_rate_limiter.h + * Message rate limiter definition. + * + * @author Anton Babushkin */ #ifndef MAVLINK_RATE_LIMITER_H_ @@ -11,7 +44,8 @@ #include -class MavlinkRateLimiter { +class MavlinkRateLimiter +{ private: hrt_abstime _last_sent; hrt_abstime _interval; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a3546e954..f85773ae0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -566,8 +566,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; /* go back to -PI..PI */ - if (heading_rad > M_PI_F) + if (heading_rad > M_PI_F) { heading_rad -= 2.0f * M_PI_F; + } hil_gps.timestamp_velocity = gps.time_usec; hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m @@ -606,7 +607,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } - // publish global position + // publish global position if (pub_hil_global_pos > 0) { orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); // global position packet @@ -627,10 +628,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) if (pub_hil_local_pos > 0) { float x; float y; - bool landed = (float)(hil_state.alt)/1000.0f < (alt0 + 0.1f); // XXX improve? - double lat = hil_state.lat*1e-7; - double lon = hil_state.lon*1e-7; - map_projection_project(lat, lon, &x, &y); + bool landed = (float)(hil_state.alt) / 1000.0f < (alt0 + 0.1f); // XXX improve? + double lat = hil_state.lat * 1e-7; + double lon = hil_state.lon * 1e-7; + map_projection_project(lat, lon, &x, &y); hil_local_pos.timestamp = timestamp; hil_local_pos.xy_valid = true; hil_local_pos.z_valid = true; @@ -638,10 +639,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) hil_local_pos.v_z_valid = true; hil_local_pos.x = x; hil_local_pos.y = y; - hil_local_pos.z = alt0 - hil_state.alt/1000.0f; - hil_local_pos.vx = hil_state.vx/100.0f; - hil_local_pos.vy = hil_state.vy/100.0f; - hil_local_pos.vz = hil_state.vz/100.0f; + hil_local_pos.z = alt0 - hil_state.alt / 1000.0f; + hil_local_pos.vx = hil_state.vx / 100.0f; + hil_local_pos.vy = hil_state.vy / 100.0f; + hil_local_pos.vz = hil_state.vz / 100.0f; hil_local_pos.yaw = hil_attitude.yaw; hil_local_pos.xy_global = true; hil_local_pos.z_global = true; @@ -651,6 +652,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) hil_local_pos.ref_alt = alt0; hil_local_pos.landed = landed; orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos); + } else { pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); lat0 = hil_state.lat; @@ -661,12 +663,13 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); - math::Matrix<3,3> C_nb = q.to_dcm(); + math::Matrix<3, 3> C_nb = q.to_dcm(); math::Vector<3> euler = C_nb.to_euler(); /* set rotation matrix */ - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) { hil_attitude.R[i][j] = C_nb(i, j); + } hil_attitude.R_valid = true; @@ -841,9 +844,9 @@ void MavlinkReceiver::print_status() } -void * MavlinkReceiver::start_helper(void *context) +void *MavlinkReceiver::start_helper(void *context) { - MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink*)context); + MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); return rcv->receive_thread(NULL); } @@ -865,7 +868,7 @@ MavlinkReceiver::receive_start(Mavlink *parent) pthread_attr_setstacksize(&receiveloop_attr, 3000); pthread_t thread; - pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void*)parent); + pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); pthread_attr_destroy(&receiveloop_attr); return thread; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index fca5de917..199e42689 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -97,13 +97,13 @@ public: static pthread_t receive_start(Mavlink *parent); - static void * start_helper(void *context); + static void *start_helper(void *context); private: perf_counter_t _loop_perf; /**< loop performance counter */ - Mavlink* _mavlink; + Mavlink *_mavlink; void handle_message(mavlink_message_t *msg); void *receive_thread(void *arg); diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 703f74b4c..869495098 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -1,8 +1,41 @@ -/* - * mavlink_stream.cpp +/**************************************************************************** * - * Created on: 24.02.2014 - * Author: ton + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_stream.cpp + * Mavlink messages stream implementation. + * + * @author Anton Babushkin */ #include @@ -43,6 +76,7 @@ int MavlinkStream::update(const hrt_abstime t) { uint64_t dt = t - _last_sent; + if (dt > 0 && dt >= _interval) { /* interval expired, send message */ send(t); diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 9f175adbe..135e1bce0 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -1,8 +1,41 @@ -/* - * mavlink_stream.h +/**************************************************************************** * - * Created on: 24.02.2014 - * Author: ton + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_stream.cpp + * Mavlink messages stream definition. + * + * @author Anton Babushkin */ #ifndef MAVLINK_STREAM_H_ @@ -15,7 +48,8 @@ class MavlinkStream; #include "mavlink_main.h" -class MavlinkStream { +class MavlinkStream +{ private: hrt_abstime _last_sent; -- cgit v1.2.3 From 1b8004cd8ecf7824584aac9e7fed447714feb716 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 16:43:04 +0400 Subject: mavlink: add new streams in main loop, minor cleanup and fixes --- ROMFS/px4fmu_common/init.d/rcS | 36 ++++++----- src/modules/mavlink/mavlink_main.cpp | 76 +++++++++++++++++++----- src/modules/mavlink/mavlink_main.h | 6 +- src/modules/mavlink/mavlink_messages.cpp | 52 ++++++++-------- src/modules/mavlink/mavlink_orb_subscription.cpp | 9 +-- src/modules/mavlink/mavlink_orb_subscription.h | 4 +- 6 files changed, 121 insertions(+), 62 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c3065b6fc..17f7dd077 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -117,6 +117,7 @@ then set PWM_MAX none set MKBLCTRL_MODE none set FMU_MODE pwm + set MAVLINK_FLAGS default set MAV_TYPE none # @@ -381,26 +382,33 @@ then # set EXIT_ON_END no - if [ $HIL == yes ] + if [ $MAVLINK_FLAGS == default ] then - sleep 1 - mavlink start -b 230400 -d /dev/ttyACM0 - usleep 5000 - else - if [ $TTYS1_BUSY == yes ] + if [ $HIL == yes ] then - # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - mavlink start -r 1000 -d /dev/ttyS0 + sleep 1 + set MAVLINK_FLAGS "-d 10000 -d /dev/ttyACM0" usleep 5000 - - # Exit from nsh to free port for mavlink - set EXIT_ON_END yes else - # Start MAVLink on default port: ttyS1 - mavlink start -r 1000 - usleep 5000 + # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s + if [ $TTYS1_BUSY == yes ] + then + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else + set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0" + usleep 5000 + + # Exit from nsh to free port for mavlink + set EXIT_ON_END yes + else + # Start MAVLink on default port: ttyS1 + set MAVLINK_FLAGS "-r 1000" + usleep 5000 + fi fi fi + + mavlink start $MAVLINK_FLAGS + usleep 5000 # # Start the datamanager diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c97786553..b996413a8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -171,6 +171,9 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), mission_pub(-1), + mavlink_param_queue_index(0), + _subscribe_to_stream(nullptr), + _subscribe_to_stream_rate(0.0f), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) @@ -301,6 +304,7 @@ Mavlink::destroy_all_instances() while (inst_to_del->thread_running) { printf("."); + fflush(stdout); usleep(10000); iterations++; @@ -1443,7 +1447,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) } } -MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, const size_t size) +MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) { /* check if already subscribed to this topic */ MavlinkOrbSubscription *sub; @@ -1456,7 +1460,7 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, cons } /* add new subscription */ - MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, size); + MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic); LL_APPEND(_subscriptions, sub_new); @@ -1509,6 +1513,34 @@ Mavlink::configure_stream(const char *stream_name, const float rate) return ERROR; } +void +Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) +{ + /* orb subscription must be done from the main thread, + * set _subscribe_to_stream and _subscribe_to_stream_rate fields + * which polled in mavlink main loop */ + if (!_task_should_exit) { + /* wait for previous subscription completion */ + while (_subscribe_to_stream != nullptr) { + usleep(MAIN_LOOP_DELAY / 2); + } + + /* copy stream name */ + unsigned n = strlen(stream_name) + 1; + char *s = new char[n]; + memcpy(s, stream_name, n); + + /* set subscription task */ + _subscribe_to_stream_rate = rate; + _subscribe_to_stream = s; + + /* wait for subscription */ + do { + usleep(MAIN_LOOP_DELAY / 2); + } while (_subscribe_to_stream != nullptr); + } +} + int Mavlink::task_main(int argc, char *argv[]) { @@ -1686,8 +1718,8 @@ Mavlink::task_main(int argc, char *argv[]) thread_running = true; - MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s)); - MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s)); + MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); + MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data(); @@ -1726,6 +1758,9 @@ Mavlink::task_main(int argc, char *argv[]) break; } + /* don't send parameters on startup without request */ + mavlink_param_queue_index = param_count(); + MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult); MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult); @@ -1752,6 +1787,24 @@ Mavlink::task_main(int argc, char *argv[]) } } + /* check for requested subscriptions */ + if (_subscribe_to_stream != nullptr) { + if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { + if (_subscribe_to_stream_rate > 0.0f) { + warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, device_name, _subscribe_to_stream_rate); + + } else { + warnx("stream %s on device %s disabled", _subscribe_to_stream, device_name); + } + + } else { + warnx("stream %s not found", _subscribe_to_stream, device_name); + } + delete _subscribe_to_stream; + _subscribe_to_stream = nullptr; + } + + /* update streams */ MavlinkStream *stream; LL_FOREACH(_streams, stream) { stream->update(t); @@ -1794,6 +1847,9 @@ Mavlink::task_main(int argc, char *argv[]) perf_end(_loop_perf); } + delete _subscribe_to_stream; + _subscribe_to_stream = nullptr; + /* wait for threads to complete */ pthread_join(receive_thread, NULL); @@ -1911,17 +1967,7 @@ Mavlink::stream(int argc, char *argv[]) Mavlink *inst = get_instance_for_device(device_name); if (inst != nullptr) { - if (OK == inst->configure_stream(stream_name, rate)) { - if (rate > 0.0f) { - warnx("stream %s on device %s enabled with rate %.1f Hz", stream_name, device_name, rate); - - } else { - warnx("stream %s on device %s disabled", stream_name, device_name); - } - - } else { - warnx("stream %s not found", stream_name, device_name); - } + inst->configure_stream_threadsafe(stream_name, rate); } else { errx(1, "mavlink for device %s is not running", device_name); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 532c9bcee..ebea53d52 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -180,7 +180,7 @@ public: */ int set_hil_enabled(bool hil_enabled); - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, size_t size); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); mavlink_channel_t get_channel(); @@ -235,6 +235,9 @@ private: bool mavlink_link_termination_allowed; + char * _subscribe_to_stream; + float _subscribe_to_stream_rate; + /** * Send one parameter. * @@ -296,6 +299,7 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); int configure_stream(const char *stream_name, const float rate); + void configure_stream_threadsafe(const char *stream_name, const float rate); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 820faae1c..8be7d76d7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -207,10 +207,10 @@ private: protected: void subscribe(Mavlink *mavlink) { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); status = (struct vehicle_status_s *)status_sub->get_data(); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } @@ -255,7 +255,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); status = (struct vehicle_status_s *)status_sub->get_data(); } @@ -310,7 +310,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined), sizeof(struct sensor_combined_s)); + sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); sensor = (struct sensor_combined_s *)sensor_sub->get_data(); } @@ -376,7 +376,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); att = (struct vehicle_attitude_s *)att_sub->get_data(); } @@ -412,7 +412,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); att = (struct vehicle_attitude_s *)att_sub->get_data(); } @@ -465,19 +465,19 @@ private: protected: void subscribe(Mavlink *mavlink) { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); att = (struct vehicle_attitude_s *)att_sub->get_data(); - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s)); + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); pos = (struct vehicle_global_position_s *)pos_sub->get_data(); - armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed), sizeof(struct actuator_armed_s)); + armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed)); armed = (struct actuator_armed_s *)armed_sub->get_data(); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0), sizeof(struct actuator_controls_s)); + act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); act = (struct actuator_controls_s *)act_sub->get_data(); - airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed), sizeof(struct airspeed_s)); + airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); airspeed = (struct airspeed_s *)airspeed_sub->get_data(); } @@ -524,7 +524,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position), sizeof(struct vehicle_gps_position_s)); + gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); gps = (struct vehicle_gps_position_s *)gps_sub->get_data(); } @@ -570,10 +570,10 @@ private: protected: void subscribe(Mavlink *mavlink) { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s)); + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); pos = (struct vehicle_global_position_s *)pos_sub->get_data(); - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s)); + home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); home = (struct home_position_s *)home_sub->get_data(); } @@ -616,7 +616,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position), sizeof(struct vehicle_local_position_s)); + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); pos = (struct vehicle_local_position_s *)pos_sub->get_data(); } @@ -656,7 +656,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s)); + home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); home = (struct home_position_s *)home_sub->get_data(); } @@ -707,7 +707,7 @@ protected: ORB_ID(actuator_outputs_3) }; - act_sub = mavlink->add_orb_subscription(act_topics[_n], sizeof(struct actuator_outputs_s)); + act_sub = mavlink->add_orb_subscription(act_topics[_n]); act = (struct actuator_outputs_s *)act_sub->get_data(); } @@ -756,13 +756,13 @@ private: protected: void subscribe(Mavlink *mavlink) { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); status = (struct vehicle_status_s *)status_sub->get_data(); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0), sizeof(struct actuator_outputs_s)); + act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); act = (struct actuator_outputs_s *)act_sub->get_data(); } @@ -861,7 +861,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } @@ -899,7 +899,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint), sizeof(vehicle_local_position_setpoint_s)); + pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data(); } @@ -937,7 +937,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint), sizeof(vehicle_attitude_setpoint_s)); + att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); } @@ -975,7 +975,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint), sizeof(vehicle_rates_setpoint_s)); + att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data(); } @@ -1013,7 +1013,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc), sizeof(struct rc_input_values)); + rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); rc = (struct rc_input_values *)rc_sub->get_data(); } @@ -1062,7 +1062,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint), sizeof(struct manual_control_setpoint_s)); + manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); manual = (struct manual_control_setpoint_s *)manual_sub->get_data(); } diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index e1208bca9..6279e5366 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -42,13 +42,14 @@ #include #include #include +#include #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, size_t size) : _topic(topic), _last_check(0), next(nullptr) +MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _topic(topic), _last_check(0), next(nullptr) { - _data = malloc(size); - memset(_data, 0, size); + _data = malloc(topic->o_size); + memset(_data, 0, topic->o_size); _fd = orb_subscribe(_topic); } @@ -58,7 +59,7 @@ MavlinkOrbSubscription::~MavlinkOrbSubscription() free(_data); } -const struct orb_metadata * +const orb_id_t MavlinkOrbSubscription::get_topic() { return _topic; diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 3cf33ccef..eacc27034 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -50,12 +50,12 @@ class MavlinkOrbSubscription public: MavlinkOrbSubscription *next; - MavlinkOrbSubscription(const orb_id_t topic, size_t size); + MavlinkOrbSubscription(const orb_id_t topic); ~MavlinkOrbSubscription(); bool update(const hrt_abstime t); void *get_data(); - const struct orb_metadata *get_topic(); + const orb_id_t get_topic(); private: const orb_id_t _topic; -- cgit v1.2.3 From 624ff010185c16dffacc8d000aae2748ff1cd0a1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Mar 2014 12:53:46 +0400 Subject: mavlink: HIL fixes, send 0 when disarmed --- src/modules/mavlink/mavlink_messages.cpp | 92 +++++++++++++------------------- 1 file changed, 36 insertions(+), 56 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 8be7d76d7..c22289772 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -778,65 +778,45 @@ protected: uint32_t mavlink_custom_mode; get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - /* HIL message as per MAVLink spec */ + /* set number of valid outputs depending on vehicle type */ + unsigned n; + switch (mavlink_system.type) { + case MAV_TYPE_QUADROTOR: + n = 4; + break; + + case MAV_TYPE_HEXAROTOR: + n = 6; + break; + + default: + n = 8; + break; + } /* scale / assign outputs depending on system type */ - if (mavlink_system.type == MAV_TYPE_QUADROTOR) { - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - -1, - -1, - mavlink_base_mode, - 0); - - } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - ((act->output[4] - 900.0f) / 600.0f) / 2.0f, - ((act->output[5] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - mavlink_base_mode, - 0); - - } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - ((act->output[4] - 900.0f) / 600.0f) / 2.0f, - ((act->output[5] - 900.0f) / 600.0f) / 2.0f, - ((act->output[6] - 900.0f) / 600.0f) / 2.0f, - ((act->output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_base_mode, - 0); - - } else { - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - (act->output[0] - 1500.0f) / 500.0f, - (act->output[1] - 1500.0f) / 500.0f, - (act->output[2] - 1500.0f) / 500.0f, - (act->output[3] - 1000.0f) / 1000.0f, - (act->output[4] - 1500.0f) / 500.0f, - (act->output[5] - 1500.0f) / 500.0f, - (act->output[6] - 1500.0f) / 500.0f, - (act->output[7] - 1500.0f) / 500.0f, - mavlink_base_mode, - 0); + float out[8]; + + for (unsigned i = 0; i < 8; i++) { + if (i < n) { + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + /* scale fake PWM out 900..1200 us to 0..1*/ + out[i] = (act->output[i] - 900.0f) / 1200.0f; + + } else { + /* send 0 when disarmed */ + out[i] = 0.0f; + } + } else { + out[i] = -1.0f; + } } + + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } }; -- cgit v1.2.3 From 14ddf1804aa9d698724fda62288b210fd850d2ac Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 00:48:20 +0400 Subject: mavlink: code style fixed --- src/modules/mavlink/mavlink_messages.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c22289772..d4c77d1fd 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -780,6 +780,7 @@ protected: /* set number of valid outputs depending on vehicle type */ unsigned n; + switch (mavlink_system.type) { case MAV_TYPE_QUADROTOR: n = 4; @@ -807,6 +808,7 @@ protected: /* send 0 when disarmed */ out[i] = 0.0f; } + } else { out[i] = -1.0f; } -- cgit v1.2.3 From 190eb6205dc3e610d223878c4b85a8e587fc6323 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 11:45:58 +0400 Subject: mavlink: OPTICAL_FLOW stream implemented --- src/modules/mavlink/mavlink_messages.cpp | 51 +++++++++++++++++++++++++------- 1 file changed, 40 insertions(+), 11 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index d4c77d1fd..7475160d5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1063,6 +1063,45 @@ protected: }; +class MavlinkStreamOpticalFlow : public MavlinkStream +{ +public: + const char *get_name() + { + return "OPTICAL_FLOW"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamOpticalFlow(); + } + +private: + MavlinkOrbSubscription *flow_sub; + struct optical_flow_s *flow; + +protected: + void subscribe(Mavlink *mavlink) + { + flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); + flow = (struct optical_flow_s *)flow_sub->get_data(); + } + + void send(const hrt_abstime t) + { + flow_sub->update(t); + + mavlink_msg_optical_flow_send(_channel, + flow->timestamp, + flow->sensor_id, + flow->flow_raw_x, flow->flow_raw_y, + flow->flow_comp_x_m, flow->flow_comp_y_m, + flow->quality, + flow->ground_distance_m); + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -1085,6 +1124,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamRollPitchYawRatesThrustSetpoint(), new MavlinkStreamRCChannelsRaw(), new MavlinkStreamManualControl(), + new MavlinkStreamOpticalFlow(), nullptr }; @@ -1143,17 +1183,6 @@ MavlinkStream *streams_list[] = { //} // //void -//MavlinkOrbListener::l_optical_flow(const struct listener *l) -//{ -// struct optical_flow_s flow; -// -// orb_copy(ORB_ID(optical_flow), l->mavlink->get_subs()->optical_flow, &flow); -// -// mavlink_msg_optical_flow_send(l->mavlink->get_chan(), flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, -// flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m); -//} -// -//void //MavlinkOrbListener::l_nav_cap(const struct listener *l) //{ // -- cgit v1.2.3 From e1742eea7d951aa704665c2b5db4d1c6788aafa0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 13:03:17 +0400 Subject: mavlink: code style fixed --- src/modules/mavlink/mavlink_messages.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7475160d5..4d83afe82 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1092,12 +1092,12 @@ protected: flow_sub->update(t); mavlink_msg_optical_flow_send(_channel, - flow->timestamp, - flow->sensor_id, - flow->flow_raw_x, flow->flow_raw_y, - flow->flow_comp_x_m, flow->flow_comp_y_m, - flow->quality, - flow->ground_distance_m); + flow->timestamp, + flow->sensor_id, + flow->flow_raw_x, flow->flow_raw_y, + flow->flow_comp_x_m, flow->flow_comp_y_m, + flow->quality, + flow->ground_distance_m); } }; -- cgit v1.2.3 From 3874bca2084bb88dcd739b309bd4a7929db3b417 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 13:48:33 +0100 Subject: mavlink: Only send messages when we have updates for them. --- src/modules/mavlink/mavlink_messages.cpp | 472 ++++++++++++----------- src/modules/mavlink/mavlink_orb_subscription.cpp | 21 +- src/modules/mavlink/mavlink_orb_subscription.h | 9 + src/modules/mavlink/mavlink_receiver.cpp | 9 +- 4 files changed, 281 insertions(+), 230 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4d83afe82..7d388f88d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -216,8 +216,8 @@ protected: void send(const hrt_abstime t) { - status_sub->update(t); - pos_sp_triplet_sub->update(t); + (void)status_sub->update(t); + (void)pos_sp_triplet_sub->update(t); uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; @@ -261,22 +261,23 @@ protected: void send(const hrt_abstime t) { - status_sub->update(t); - - mavlink_msg_sys_status_send(_channel, - status->onboard_control_sensors_present, - status->onboard_control_sensors_enabled, - status->onboard_control_sensors_health, - status->load * 1000.0f, - status->battery_voltage * 1000.0f, - status->battery_current * 1000.0f, - status->battery_remaining, - status->drop_rate_comm, - status->errors_comm, - status->errors_count1, - status->errors_count2, - status->errors_count3, - status->errors_count4); + if (status_sub->update(t)) { + + mavlink_msg_sys_status_send(_channel, + status->onboard_control_sensors_present, + status->onboard_control_sensors_enabled, + status->onboard_control_sensors_health, + status->load * 1000.0f, + status->battery_voltage * 1000.0f, + status->battery_current * 1000.0f, + status->battery_remaining, + status->drop_rate_comm, + status->errors_comm, + status->errors_count1, + status->errors_count2, + status->errors_count3, + status->errors_count4); + } } }; @@ -284,7 +285,7 @@ protected: class MavlinkStreamHighresIMU : public MavlinkStream { public: - MavlinkStreamHighresIMU() : MavlinkStream(), accel_counter(0), gyro_counter(0), mag_counter(0), baro_counter(0) + MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0) { } @@ -302,10 +303,10 @@ private: MavlinkOrbSubscription *sensor_sub; struct sensor_combined_s *sensor; - uint32_t accel_counter; - uint32_t gyro_counter; - uint32_t mag_counter; - uint32_t baro_counter; + uint64_t accel_timestamp; + uint64_t gyro_timestamp; + uint64_t mag_timestamp; + uint64_t baro_timestamp; protected: void subscribe(Mavlink *mavlink) @@ -316,42 +317,43 @@ protected: void send(const hrt_abstime t) { - sensor_sub->update(t); + if (sensor_sub->update(t)) { - uint16_t fields_updated = 0; + uint16_t fields_updated = 0; - if (accel_counter != sensor->accelerometer_counter) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_counter = sensor->accelerometer_counter; - } + if (accel_timestamp != sensor->accelerometer_timestamp) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + accel_timestamp = sensor->accelerometer_timestamp; + } - if (gyro_counter != sensor->gyro_counter) { - /* mark second group dimensions as changed */ - fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_counter = sensor->gyro_counter; - } + if (gyro_timestamp != sensor->timestamp) { + /* mark second group dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_timestamp = sensor->timestamp; + } - if (mag_counter != sensor->magnetometer_counter) { - /* mark third group dimensions as changed */ - fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_counter = sensor->magnetometer_counter; - } + if (mag_timestamp != sensor->magnetometer_timestamp) { + /* mark third group dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_timestamp = sensor->magnetometer_timestamp; + } - if (baro_counter != sensor->baro_counter) { - /* mark last group dimensions as changed */ - fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_counter = sensor->baro_counter; - } + if (baro_timestamp != sensor->baro_timestamp) { + /* mark last group dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_timestamp = sensor->baro_timestamp; + } - mavlink_msg_highres_imu_send(_channel, - sensor->timestamp, - sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], - sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], - sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], - sensor->baro_pres_mbar, sensor->differential_pressure_pa, - sensor->baro_alt_meter, sensor->baro_temp_celcius, - fields_updated); + mavlink_msg_highres_imu_send(_channel, + sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); + } } }; @@ -382,12 +384,13 @@ protected: void send(const hrt_abstime t) { - att_sub->update(t); + if (att_sub->update(t)) { - mavlink_msg_attitude_send(_channel, - att->timestamp / 1000, - att->roll, att->pitch, att->yaw, - att->rollspeed, att->pitchspeed, att->yawspeed); + mavlink_msg_attitude_send(_channel, + att->timestamp / 1000, + att->roll, att->pitch, att->yaw, + att->rollspeed, att->pitchspeed, att->yawspeed); + } } }; @@ -418,17 +421,18 @@ protected: void send(const hrt_abstime t) { - att_sub->update(t); - - mavlink_msg_attitude_quaternion_send(_channel, - att->timestamp / 1000, - att->q[0], - att->q[1], - att->q[2], - att->q[3], - att->rollspeed, - att->pitchspeed, - att->yawspeed); + if (att_sub->update(t)) { + + mavlink_msg_attitude_quaternion_send(_channel, + att->timestamp / 1000, + att->q[0], + att->q[1], + att->q[2], + att->q[3], + att->rollspeed, + att->pitchspeed, + att->yawspeed); + } } }; @@ -483,23 +487,26 @@ protected: void send(const hrt_abstime t) { - att_sub->update(t); - pos_sub->update(t); - armed_sub->update(t); - act_sub->update(t); - airspeed_sub->update(t); - - float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e); - uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F; - float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f; - - mavlink_msg_vfr_hud_send(_channel, - airspeed->true_airspeed_m_s, - groundspeed, - heading, - throttle, - pos->alt, - -pos->vel_d); + bool updated = att_sub->update(t); + updated |= pos_sub->update(t); + updated |= armed_sub->update(t); + updated |= act_sub->update(t); + updated |= airspeed_sub->update(t); + + if (updated) { + + float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e); + uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F; + float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f; + + mavlink_msg_vfr_hud_send(_channel, + airspeed->true_airspeed_m_s, + groundspeed, + heading, + throttle, + pos->alt, + -pos->vel_d); + } } }; @@ -530,19 +537,20 @@ protected: void send(const hrt_abstime t) { - gps_sub->update(t); - - mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph_m), - cm_uint16_from_m_float(gps->epv_m), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); + if (gps_sub->update(t)) { + + mavlink_msg_gps_raw_int_send(_channel, + gps->timestamp_position, + gps->fix_type, + gps->lat, + gps->lon, + gps->alt, + cm_uint16_from_m_float(gps->eph_m), + cm_uint16_from_m_float(gps->epv_m), + gps->vel_m_s * 100.0f, + _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps->satellites_visible); + } } }; @@ -579,10 +587,11 @@ protected: void send(const hrt_abstime t) { - pos_sub->update(t); - home_sub->update(t); + bool updated = pos_sub->update(t); + updated |= home_sub->update(t); - mavlink_msg_global_position_int_send(_channel, + if (updated) { + mavlink_msg_global_position_int_send(_channel, pos->timestamp / 1000, pos->lat * 1e7, pos->lon * 1e7, @@ -592,6 +601,7 @@ protected: pos->vel_e * 100.0f, pos->vel_d * 100.0f, _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + } } }; @@ -622,16 +632,17 @@ protected: void send(const hrt_abstime t) { - pos_sub->update(t); + if (pos_sub->update(t)) { - mavlink_msg_local_position_ned_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->vx, - pos->vy, - pos->vz); + mavlink_msg_local_position_ned_send(_channel, + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->vx, + pos->vy, + pos->vz); + } } }; @@ -662,12 +673,17 @@ protected: void send(const hrt_abstime t) { - home_sub->update(t); - mavlink_msg_gps_global_origin_send(_channel, - (int32_t)(home->lat * 1e7), - (int32_t)(home->lon * 1e7), - (int32_t)(home->alt) * 1000.0f); + /* we're sending the GPS home periodically to ensure the + * the GCS does pick it up at one point */ + if (home_sub->is_published()) { + home_sub->update(t); + + mavlink_msg_gps_global_origin_send(_channel, + (int32_t)(home->lat * 1e7), + (int32_t)(home->lon * 1e7), + (int32_t)(home->alt) * 1000.0f); + } } }; @@ -713,19 +729,20 @@ protected: void send(const hrt_abstime t) { - act_sub->update(t); - - mavlink_msg_servo_output_raw_send(_channel, - act->timestamp / 1000, - _n, - act->output[0], - act->output[1], - act->output[2], - act->output[3], - act->output[4], - act->output[5], - act->output[6], - act->output[7]); + if (act_sub->update(t)) { + + mavlink_msg_servo_output_raw_send(_channel, + act->timestamp / 1000, + _n, + act->output[0], + act->output[1], + act->output[2], + act->output[3], + act->output[4], + act->output[5], + act->output[6], + act->output[7]); + } } }; @@ -768,57 +785,60 @@ protected: void send(const hrt_abstime t) { - status_sub->update(t); - pos_sp_triplet_sub->update(t); - act_sub->update(t); + bool updated = status_sub->update(t); + updated |= pos_sp_triplet_sub->update(t); + updated |= act_sub->update(t); - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state; - uint8_t mavlink_base_mode; - uint32_t mavlink_custom_mode; - get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + if (updated) { - /* set number of valid outputs depending on vehicle type */ - unsigned n; + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state; + uint8_t mavlink_base_mode; + uint32_t mavlink_custom_mode; + get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - switch (mavlink_system.type) { - case MAV_TYPE_QUADROTOR: - n = 4; - break; + /* set number of valid outputs depending on vehicle type */ + unsigned n; - case MAV_TYPE_HEXAROTOR: - n = 6; - break; + switch (mavlink_system.type) { + case MAV_TYPE_QUADROTOR: + n = 4; + break; - default: - n = 8; - break; - } + case MAV_TYPE_HEXAROTOR: + n = 6; + break; + + default: + n = 8; + break; + } + + /* scale / assign outputs depending on system type */ + float out[8]; - /* scale / assign outputs depending on system type */ - float out[8]; + for (unsigned i = 0; i < 8; i++) { + if (i < n) { + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + /* scale fake PWM out 900..1200 us to 0..1*/ + out[i] = (act->output[i] - 900.0f) / 1200.0f; - for (unsigned i = 0; i < 8; i++) { - if (i < n) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - /* scale fake PWM out 900..1200 us to 0..1*/ - out[i] = (act->output[i] - 900.0f) / 1200.0f; + } else { + /* send 0 when disarmed */ + out[i] = 0.0f; + } } else { - /* send 0 when disarmed */ - out[i] = 0.0f; + out[i] = -1.0f; } - - } else { - out[i] = -1.0f; } - } - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); + } } }; @@ -849,14 +869,15 @@ protected: void send(const hrt_abstime t) { - pos_sp_triplet_sub->update(t); + if (pos_sp_triplet_sub->update(t)) { - mavlink_msg_global_position_setpoint_int_send(_channel, - MAV_FRAME_GLOBAL, - (int32_t)(pos_sp_triplet->current.lat * 1e7), - (int32_t)(pos_sp_triplet->current.lon * 1e7), - (int32_t)(pos_sp_triplet->current.alt * 1000), - (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); + mavlink_msg_global_position_setpoint_int_send(_channel, + MAV_FRAME_GLOBAL, + (int32_t)(pos_sp_triplet->current.lat * 1e7), + (int32_t)(pos_sp_triplet->current.lon * 1e7), + (int32_t)(pos_sp_triplet->current.alt * 1000), + (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); + } } }; @@ -925,14 +946,15 @@ protected: void send(const hrt_abstime t) { - att_sp_sub->update(t); + if (att_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, - att_sp->timestamp / 1000, - att_sp->roll_body, - att_sp->pitch_body, - att_sp->yaw_body, - att_sp->thrust); + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, + att_sp->timestamp / 1000, + att_sp->roll_body, + att_sp->pitch_body, + att_sp->yaw_body, + att_sp->thrust); + } } }; @@ -963,14 +985,15 @@ protected: void send(const hrt_abstime t) { - att_rates_sp_sub->update(t); + if (att_rates_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, - att_rates_sp->timestamp / 1000, - att_rates_sp->roll, - att_rates_sp->pitch, - att_rates_sp->yaw, - att_rates_sp->thrust); + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, + att_rates_sp->timestamp / 1000, + att_rates_sp->roll, + att_rates_sp->pitch, + att_rates_sp->yaw, + att_rates_sp->thrust); + } } }; @@ -1001,24 +1024,25 @@ protected: void send(const hrt_abstime t) { - rc_sub->update(t); - - const unsigned port_width = 8; - - for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(_channel, - rc->timestamp_publication / 1000, - i, - (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, - rc->rssi); + if (rc_sub->update(t)) { + + const unsigned port_width = 8; + + for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { + /* Channels are sent in MAVLink main loop at a fixed interval */ + mavlink_msg_rc_channels_raw_send(_channel, + rc->timestamp_publication / 1000, + i, + (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, + rc->rssi); + } } } }; @@ -1050,15 +1074,16 @@ protected: void send(const hrt_abstime t) { - manual_sub->update(t); + if (manual_sub->update(t)) { - mavlink_msg_manual_control_send(_channel, - mavlink_system.sysid, - manual->roll * 1000, - manual->pitch * 1000, - manual->yaw * 1000, - manual->throttle * 1000, - 0); + mavlink_msg_manual_control_send(_channel, + mavlink_system.sysid, + manual->roll * 1000, + manual->pitch * 1000, + manual->yaw * 1000, + manual->throttle * 1000, + 0); + } } }; @@ -1089,15 +1114,16 @@ protected: void send(const hrt_abstime t) { - flow_sub->update(t); + if (flow_sub->update(t)) { - mavlink_msg_optical_flow_send(_channel, - flow->timestamp, - flow->sensor_id, - flow->flow_raw_x, flow->flow_raw_y, - flow->flow_comp_x_m, flow->flow_comp_y_m, - flow->quality, - flow->ground_distance_m); + mavlink_msg_optical_flow_send(_channel, + flow->timestamp, + flow->sensor_id, + flow->flow_raw_x, flow->flow_raw_y, + flow->flow_comp_x_m, flow->flow_comp_y_m, + flow->quality, + flow->ground_distance_m); + } } }; diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 6279e5366..996318468 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -46,11 +46,15 @@ #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _topic(topic), _last_check(0), next(nullptr) +MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : + _fd(orb_subscribe(_topic)), + _published(false), + _topic(topic), + _last_check(0), + next(nullptr) { _data = malloc(topic->o_size); memset(_data, 0, topic->o_size); - _fd = orb_subscribe(_topic); } MavlinkOrbSubscription::~MavlinkOrbSubscription() @@ -87,3 +91,16 @@ MavlinkOrbSubscription::update(const hrt_abstime t) return false; } + +bool +MavlinkOrbSubscription::is_published() +{ + bool updated; + orb_check(_fd, &updated); + + if (updated) { + _published = true; + } + + return _published; +} diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index eacc27034..42d47e96e 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -54,12 +54,21 @@ public: ~MavlinkOrbSubscription(); bool update(const hrt_abstime t); + + /** + * Check if the topic has been published. + * + * This call will return true if the topic was ever published. + * @param true if the topic has been published at least once. + */ + bool is_published(); void *get_data(); const orb_id_t get_topic(); private: const orb_id_t _topic; int _fd; + bool _published; void *_data; hrt_abstime _last_check; }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c222a3ddf..2ce86396c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -586,7 +586,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.gyro_rad_s[0] = imu.xgyro; hil_sensors.gyro_rad_s[1] = imu.ygyro; hil_sensors.gyro_rad_s[2] = imu.zgyro; - hil_sensors.gyro_counter = _hil_counter; hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; @@ -596,7 +595,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.accelerometer_m_s2[2] = imu.zacc; hil_sensors.accelerometer_mode = 0; // TODO what is this? hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 - hil_sensors.accelerometer_counter = _hil_counter; + hil_sensors.accelerometer_timestamp = timestamp; hil_sensors.adc_voltage_v[0] = 0.0f; hil_sensors.adc_voltage_v[1] = 0.0f; @@ -611,15 +610,15 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.magnetometer_range_ga = 32.7f; // int16 hil_sensors.magnetometer_mode = 0; // TODO what is this hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; - hil_sensors.magnetometer_counter = _hil_counter; + hil_sensors.magnetometer_timestamp = timestamp; hil_sensors.baro_pres_mbar = imu.abs_pressure; hil_sensors.baro_alt_meter = imu.pressure_alt; hil_sensors.baro_temp_celcius = imu.temperature; - hil_sensors.baro_counter = _hil_counter; + hil_sensors.baro_timestamp = timestamp; hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_counter = _hil_counter; + hil_sensors.differential_pressure_timestamp = timestamp; /* publish combined sensor topic */ if (_sensors_pub > 0) { -- cgit v1.2.3 From 8fe3475b41b76ecf07aa6cd1d73196c17b4c8ebe Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Mar 2014 20:12:12 +0100 Subject: mavlink: add onboard function for camera capture commands --- src/modules/mavlink/mavlink_main.cpp | 13 ++++++++++ src/modules/mavlink/mavlink_main.h | 3 ++- src/modules/mavlink/mavlink_messages.cpp | 44 +++++++++++++++++++++++++++++++- 3 files changed, 58 insertions(+), 2 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9c2e0178a..2457a7cae 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1653,6 +1653,8 @@ Mavlink::task_main(int argc, char *argv[]) case 'm': if (strcmp(optarg, "custom") == 0) { _mode = MAVLINK_MODE_CUSTOM; + } else if (strcmp(optarg, "camera") == 0) { + _mode = MAVLINK_MODE_CAMERA; } break; @@ -1696,6 +1698,10 @@ Mavlink::task_main(int argc, char *argv[]) warnx("mode: CUSTOM"); break; + case MAVLINK_MODE_CAMERA: + warnx("mode: CAMERA"); + break; + default: warnx("ERROR: Unknown mode"); break; @@ -1765,6 +1771,13 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); break; + case MAVLINK_MODE_CAMERA: + configure_stream("SYS_STATUS", 1.0f); + configure_stream("ATTITUDE", 20.0f); + configure_stream("GLOBAL_POSITION_INT", 20.0f); + configure_stream("CAMERA_CAPTURE", 1.0f); + break; + default: break; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index f743c2504..5a118a0ad 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -146,7 +146,8 @@ public: enum MAVLINK_MODE { MAVLINK_MODE_NORMAL = 0, - MAVLINK_MODE_CUSTOM + MAVLINK_MODE_CUSTOM, + MAVLINK_MODE_CAMERA }; void set_mode(enum MAVLINK_MODE); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7d388f88d..014b53829 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -230,7 +230,6 @@ protected: mavlink_base_mode, mavlink_custom_mode, mavlink_state); - } }; @@ -1127,6 +1126,48 @@ protected: } }; +class MavlinkStreamCameraCapture : public MavlinkStream +{ +public: + const char *get_name() + { + return "CAMERA_CAPTURE"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamCameraCapture(); + } + +private: + MavlinkOrbSubscription *status_sub; + struct vehicle_status_s *status; + +protected: + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + status = (struct vehicle_status_s *)status_sub->get_data(); + + + } + + void send(const hrt_abstime t) + { + (void)status_sub->update(t); + + if (status->arming_state == ARMING_STATE_ARMED + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + + /* send camera capture on */ + mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + } else { + /* send camera capture off */ + mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + } + } +}; + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), @@ -1151,6 +1192,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamRCChannelsRaw(), new MavlinkStreamManualControl(), new MavlinkStreamOpticalFlow(), + new MavlinkStreamCameraCapture(), nullptr }; -- cgit v1.2.3 From 2988136e7eaa1f55c41376b74b58766af9f8fcb9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 10:44:31 +0100 Subject: Implemented last missing messages, added stream config for USB, made stream config fails for non-existing mavlink links non-fatal --- ROMFS/px4fmu_common/init.d/rc.usb | 4 + src/modules/mavlink/mavlink_main.cpp | 21 ++++- src/modules/mavlink/mavlink_messages.cpp | 156 ++++++++++++++++++------------- 3 files changed, 110 insertions(+), 71 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 558be4275..2af790fc4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -6,6 +6,10 @@ echo "Starting MAVLink on this USB console" mavlink start -r 10000 -d /dev/ttyACM0 +# Enable a number of interesting streams we want via USB +mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 +mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10 +mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROL -r 30 # Exit shell to make it available to MAVLink exit diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c9f4835ba..00d906dcd 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -588,7 +588,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* setup output flow control */ if (enable_flow_control(true)) { - warnx("ERR FLOW CTRL EN"); + warnx("hardware flow control not supported"); } } @@ -1762,6 +1762,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); + configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); break; default: @@ -1951,8 +1952,17 @@ Mavlink::start(int argc, char *argv[]) // the only path to create a new instance, // this is effectively a lock on concurrent // instance starting. XXX do a real lock. - while (ic == Mavlink::instance_count()) { - ::usleep(500); + + // Sleep 500 us between each attempt + const unsigned sleeptime = 500; + + // Wait 100 ms max for the startup. + const unsigned limit = 100 * 1000 / sleeptime; + + unsigned count = 0; + while (ic == Mavlink::instance_count() && count < limit) { + ::usleep(sleeptime); + count++; } return OK; @@ -2008,7 +2018,10 @@ Mavlink::stream(int argc, char *argv[]) inst->configure_stream_threadsafe(stream_name, rate); } else { - errx(1, "mavlink for device %s is not running", device_name); + + // If the link is not running we should complain, but not fall over + // because this is so easy to get wrong and not fatal. Warning is sufficient. + errx(0, "mavlink for device %s is not running", device_name); } } else { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7d388f88d..dc01935ca 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1127,6 +1127,93 @@ protected: } }; +class MavlinkStreamAttitudeControls : public MavlinkStream +{ +public: + const char *get_name() + { + return "ATTITUDE_CONTROLS"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamAttitudeControls(); + } + +private: + MavlinkOrbSubscription *att_ctrl_sub; + struct actuator_controls_s *att_ctrl; + +protected: + void subscribe(Mavlink *mavlink) + { + att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (att_ctrl_sub->update(t)) { + + // send, add spaces so that string buffer is at least 10 chars long + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "rll ctrl ", + att_ctrl->control[0]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "ptch ctrl ", + att_ctrl->control[1]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "yaw ctrl ", + att_ctrl->control[2]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "thr ctrl ", + att_ctrl->control[3]); + } + } +}; + +class MavlinkStreamNamedValueFloat : public MavlinkStream +{ +public: + const char *get_name() + { + return "NAMED_VALUE_FLOAT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamNamedValueFloat(); + } + +private: + MavlinkOrbSubscription *debug_sub; + struct debug_key_value_s *debug; + +protected: + void subscribe(Mavlink *mavlink) + { + debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); + debug = (struct debug_key_value_s *)debug_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (debug_sub->update(t)) { + + // Enforce null termination + debug->key[sizeof(debug->key) - 1] = '\0'; + + mavlink_msg_named_value_float_send(_channel, + debug->timestamp_ms, + debug->key, + debug->value); + } + } +}; MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), @@ -1151,72 +1238,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamRCChannelsRaw(), new MavlinkStreamManualControl(), new MavlinkStreamOpticalFlow(), + new MavlinkStreamAttitudeControls(), + new MavlinkStreamNamedValueFloat(), nullptr }; - - - - - - - -// -// -// -// -// -// -//void -//MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l) -//{ -// orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { -// /* send, add spaces so that string buffer is at least 10 chars long */ -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl0 ", -// l->listener->actuators_0.control[0]); -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl1 ", -// l->listener->actuators_0.control[1]); -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl2 ", -// l->listener->actuators_0.control[2]); -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl3 ", -// l->listener->actuators_0.control[3]); -// } -//} -// -//void -//MavlinkOrbListener::l_debug_key_value(const struct listener *l) -//{ -// struct debug_key_value_s debug; -// -// orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug); -// -// /* Enforce null termination */ -// debug.key[sizeof(debug.key) - 1] = '\0'; -// -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// debug.key, -// debug.value); -//} -// -//void -//MavlinkOrbListener::l_nav_cap(const struct listener *l) -//{ -// -// orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap); -// -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// hrt_absolute_time() / 1000, -// "turn dist", -// l->listener->nav_cap.turn_distance); -// -//} -- cgit v1.2.3 From 1c49d768fbf2b69bf2500a4dc48ac2a95c0d2ba1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 21 Mar 2014 20:06:34 +0400 Subject: mavlink: code style fixed --- src/modules/mavlink/mavlink_main.cpp | 41 +++++++++++---- src/modules/mavlink/mavlink_messages.cpp | 90 +++++++++++++------------------- 2 files changed, 68 insertions(+), 63 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ff1c070cb..9a8e1e5a7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -151,8 +151,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } /* no valid instance, bail */ - if (!instance) + if (!instance) { return; + } int uart = instance->get_uart_fd(); @@ -165,12 +166,12 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length int buf_free = 0; if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { if (buf_free == 0) { if (last_write_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { + hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { warnx("DISABLING HARDWARE FLOW CONTROL"); instance->enable_flow_control(false); @@ -225,30 +226,37 @@ Mavlink::Mavlink() : case 0: _channel = MAVLINK_COMM_0; break; + case 1: _channel = MAVLINK_COMM_1; break; + case 2: _channel = MAVLINK_COMM_2; break; + case 3: _channel = MAVLINK_COMM_3; break; #ifdef MAVLINK_COMM_4 + case 4: _channel = MAVLINK_COMM_4; break; #endif #ifdef MAVLINK_COMM_5 - case 5: + + case 5: _channel = MAVLINK_COMM_5; break; #endif #ifdef MAVLINK_COMM_6 + case 6: _channel = MAVLINK_COMM_6; break; #endif + default: errx(1, "instance ID is out of range"); break; @@ -279,6 +287,7 @@ Mavlink::~Mavlink() } } while (_task_running); } + LL_DELETE(_mavlink_instances, this); } @@ -604,12 +613,16 @@ Mavlink::enable_flow_control(bool enabled) } struct termios uart_config; + int ret = tcgetattr(_uart_fd, &uart_config); + if (enabled) { uart_config.c_cflag |= CRTSCTS; + } else { uart_config.c_cflag &= ~CRTSCTS; } + ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); if (!ret) { @@ -1652,6 +1665,7 @@ Mavlink::task_main(int argc, char *argv[]) case 'm': if (strcmp(optarg, "custom") == 0) { _mode = MAVLINK_MODE_CUSTOM; + } else if (strcmp(optarg, "camera") == 0) { _mode = MAVLINK_MODE_CAMERA; } @@ -1973,6 +1987,7 @@ Mavlink::start(int argc, char *argv[]) const unsigned limit = 100 * 1000 / sleeptime; unsigned count = 0; + while (ic == Mavlink::instance_count() && count < limit) { ::usleep(sleeptime); count++; @@ -2003,20 +2018,26 @@ Mavlink::stream(int argc, char *argv[]) bool err_flag = false; int i = 0; + while (i < argc) { - if (0 == strcmp(argv[i], "-r") && i < argc - 1 ) { - rate = strtod(argv[i+1], nullptr); + if (0 == strcmp(argv[i], "-r") && i < argc - 1) { + rate = strtod(argv[i + 1], nullptr); + if (rate < 0.0f) { err_flag = true; } + i++; - } else if (0 == strcmp(argv[i], "-d") && i < argc - 1 ) { - device_name = argv[i+1]; + + } else if (0 == strcmp(argv[i], "-d") && i < argc - 1) { + device_name = argv[i + 1]; i++; - } else if (0 == strcmp(argv[i], "-s") && i < argc - 1 ) { - stream_name = argv[i+1]; + + } else if (0 == strcmp(argv[i], "-s") && i < argc - 1) { + stream_name = argv[i + 1]; i++; + } else { err_flag = true; } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a4a3c681f..037999af7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -262,7 +262,6 @@ protected: void send(const hrt_abstime t) { if (status_sub->update(t)) { - mavlink_msg_sys_status_send(_channel, status->onboard_control_sensors_present, status->onboard_control_sensors_enabled, @@ -318,7 +317,6 @@ protected: void send(const hrt_abstime t) { if (sensor_sub->update(t)) { - uint16_t fields_updated = 0; if (accel_timestamp != sensor->accelerometer_timestamp) { @@ -385,7 +383,6 @@ protected: void send(const hrt_abstime t) { if (att_sub->update(t)) { - mavlink_msg_attitude_send(_channel, att->timestamp / 1000, att->roll, att->pitch, att->yaw, @@ -422,7 +419,6 @@ protected: void send(const hrt_abstime t) { if (att_sub->update(t)) { - mavlink_msg_attitude_quaternion_send(_channel, att->timestamp / 1000, att->q[0], @@ -494,7 +490,6 @@ protected: updated |= airspeed_sub->update(t); if (updated) { - float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e); uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F; float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f; @@ -538,7 +533,6 @@ protected: void send(const hrt_abstime t) { if (gps_sub->update(t)) { - mavlink_msg_gps_raw_int_send(_channel, gps->timestamp_position, gps->fix_type, @@ -592,15 +586,15 @@ protected: if (updated) { mavlink_msg_global_position_int_send(_channel, - pos->timestamp / 1000, - pos->lat * 1e7, - pos->lon * 1e7, - pos->alt * 1000.0f, - (pos->alt - home->alt) * 1000.0f, - pos->vel_n * 100.0f, - pos->vel_e * 100.0f, - pos->vel_d * 100.0f, - _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + pos->timestamp / 1000, + pos->lat * 1e7, + pos->lon * 1e7, + pos->alt * 1000.0f, + (pos->alt - home->alt) * 1000.0f, + pos->vel_n * 100.0f, + pos->vel_e * 100.0f, + pos->vel_d * 100.0f, + _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); } } }; @@ -633,7 +627,6 @@ protected: void send(const hrt_abstime t) { if (pos_sub->update(t)) { - mavlink_msg_local_position_ned_send(_channel, pos->timestamp / 1000, pos->x, @@ -730,7 +723,6 @@ protected: void send(const hrt_abstime t) { if (act_sub->update(t)) { - mavlink_msg_servo_output_raw_send(_channel, act->timestamp / 1000, _n, @@ -790,7 +782,6 @@ protected: updated |= act_sub->update(t); if (updated) { - /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state; uint8_t mavlink_base_mode; @@ -870,7 +861,6 @@ protected: void send(const hrt_abstime t) { if (pos_sp_triplet_sub->update(t)) { - mavlink_msg_global_position_setpoint_int_send(_channel, MAV_FRAME_GLOBAL, (int32_t)(pos_sp_triplet->current.lat * 1e7), @@ -908,14 +898,14 @@ protected: void send(const hrt_abstime t) { - pos_sp_sub->update(t); - - mavlink_msg_local_position_setpoint_send(_channel, - MAV_FRAME_LOCAL_NED, - pos_sp->x, - pos_sp->y, - pos_sp->z, - pos_sp->yaw); + if (pos_sp_sub->update(t)) { + mavlink_msg_local_position_setpoint_send(_channel, + MAV_FRAME_LOCAL_NED, + pos_sp->x, + pos_sp->y, + pos_sp->z, + pos_sp->yaw); + } } }; @@ -947,7 +937,6 @@ protected: void send(const hrt_abstime t) { if (att_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, att_sp->timestamp / 1000, att_sp->roll_body, @@ -986,7 +975,6 @@ protected: void send(const hrt_abstime t) { if (att_rates_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, att_rates_sp->timestamp / 1000, att_rates_sp->roll, @@ -1025,7 +1013,6 @@ protected: void send(const hrt_abstime t) { if (rc_sub->update(t)) { - const unsigned port_width = 8; for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { @@ -1075,7 +1062,6 @@ protected: void send(const hrt_abstime t) { if (manual_sub->update(t)) { - mavlink_msg_manual_control_send(_channel, mavlink_system.sysid, manual->roll * 1000, @@ -1115,7 +1101,6 @@ protected: void send(const hrt_abstime t) { if (flow_sub->update(t)) { - mavlink_msg_optical_flow_send(_channel, flow->timestamp, flow->sensor_id, @@ -1154,24 +1139,23 @@ protected: void send(const hrt_abstime t) { if (att_ctrl_sub->update(t)) { - - // send, add spaces so that string buffer is at least 10 chars long - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "rll ctrl ", - att_ctrl->control[0]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "ptch ctrl ", - att_ctrl->control[1]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "yaw ctrl ", - att_ctrl->control[2]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "thr ctrl ", - att_ctrl->control[3]); + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "rll ctrl ", + att_ctrl->control[0]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "ptch ctrl ", + att_ctrl->control[1]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "yaw ctrl ", + att_ctrl->control[2]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "thr ctrl ", + att_ctrl->control[3]); } } }; @@ -1203,8 +1187,7 @@ protected: void send(const hrt_abstime t) { if (debug_sub->update(t)) { - - // Enforce null termination + /* enforce null termination */ debug->key[sizeof(debug->key) - 1] = '\0'; mavlink_msg_named_value_float_send(_channel, @@ -1246,10 +1229,11 @@ protected: (void)status_sub->update(t); if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + } else { /* send camera capture off */ mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); -- cgit v1.2.3 From 6c9a40b714bb311e2841289c9449f80effdf14ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Mar 2014 14:34:15 +0100 Subject: Fixed the HIL actuator range to what it should be --- ROMFS/px4fmu_common/init.d/rc.usb | 3 ++- src/modules/mavlink/mavlink_messages.cpp | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 2aeea5cbe..9f80219b1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -10,6 +10,7 @@ mavlink start -r 10000 -d /dev/ttyACM0 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10 mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30 +mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 10 # Exit shell to make it available to MAVLink -exit +exit diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 037999af7..96852bb0e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -811,8 +811,8 @@ protected: for (unsigned i = 0; i < 8; i++) { if (i < n) { if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - /* scale fake PWM out 900..1200 us to 0..1*/ - out[i] = (act->output[i] - 900.0f) / 1200.0f; + /* scale fake PWM out 900..2100 us to -1..1*/ + out[i] = (act->output[i] - 1500.0f) / 600.0f; } else { /* send 0 when disarmed */ -- cgit v1.2.3 From 2c2c4af59907a1a2dd9fb0635fd9a179f8c8860f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 25 Mar 2014 12:21:07 +0100 Subject: mavlink / HIL: Split handling of actuator feedback between fixed wing and multicopters --- src/modules/mavlink/mavlink_messages.cpp | 84 ++++++++++++++++++++------------ 1 file changed, 53 insertions(+), 31 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 96852bb0e..e292f52ba 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -71,6 +71,7 @@ #include #include #include +#include #include "mavlink_messages.h" @@ -762,6 +763,9 @@ private: MavlinkOrbSubscription *act_sub; struct actuator_outputs_s *act; + MavlinkOrbSubscription *ctrl_sub; + struct actuator_controls_s *ctrl; + protected: void subscribe(Mavlink *mavlink) { @@ -773,6 +777,9 @@ protected: act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); act = (struct actuator_outputs_s *)act_sub->get_data(); + + ctrl_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); + ctrl = (struct actuator_controls_s *)ctrl_sub->get_data(); } void send(const hrt_abstime t) @@ -780,6 +787,7 @@ protected: bool updated = status_sub->update(t); updated |= pos_sp_triplet_sub->update(t); updated |= act_sub->update(t); + updated |= ctrl_sub->update(t); if (updated) { /* translate the current syste state to mavlink state and mode */ @@ -788,47 +796,61 @@ protected: uint32_t mavlink_custom_mode; get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - /* set number of valid outputs depending on vehicle type */ - unsigned n; + if (mavlink_system.type == MAV_TYPE_QUADROTOR || + mavlink_system.type == MAV_TYPE_HEXAROTOR || + mavlink_system.type == MAV_TYPE_OCTOROTOR) { + /* set number of valid outputs depending on vehicle type */ + unsigned n; - switch (mavlink_system.type) { - case MAV_TYPE_QUADROTOR: - n = 4; - break; + switch (mavlink_system.type) { + case MAV_TYPE_QUADROTOR: + n = 4; + break; - case MAV_TYPE_HEXAROTOR: - n = 6; - break; + case MAV_TYPE_HEXAROTOR: + n = 6; + break; - default: - n = 8; - break; - } + default: + n = 8; + break; + } - /* scale / assign outputs depending on system type */ - float out[8]; + /* scale / assign outputs depending on system type */ + float out[8]; - for (unsigned i = 0; i < 8; i++) { - if (i < n) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - /* scale fake PWM out 900..2100 us to -1..1*/ - out[i] = (act->output[i] - 1500.0f) / 600.0f; + for (unsigned i = 0; i < 8; i++) { + if (i < n) { + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ + out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + + } else { + /* send 0 when disarmed */ + out[i] = 0.0f; + } } else { - /* send 0 when disarmed */ - out[i] = 0.0f; + out[i] = -1.0f; } - - } else { - out[i] = -1.0f; } - } - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); + } else { + + /* no special handling required, just send the controls */ + + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + ctrl->control[0], ctrl->control[1], ctrl->control[2], ctrl->control[3], + ctrl->control[4], ctrl->control[5], ctrl->control[6], ctrl->control[7], + mavlink_base_mode, + 0); + } } } }; -- cgit v1.2.3 From 506c16f12a8420991a0e64c616bbf5833ce1845c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 25 Mar 2014 17:37:44 +0100 Subject: Bring fixed wing HIL back to normal mode, keep multicopter unchanged --- src/modules/mavlink/mavlink_messages.cpp | 31 +++++++++++++++++++++---------- 1 file changed, 21 insertions(+), 10 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e292f52ba..4ca3840d4 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -763,9 +763,6 @@ private: MavlinkOrbSubscription *act_sub; struct actuator_outputs_s *act; - MavlinkOrbSubscription *ctrl_sub; - struct actuator_controls_s *ctrl; - protected: void subscribe(Mavlink *mavlink) { @@ -777,9 +774,6 @@ protected: act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); act = (struct actuator_outputs_s *)act_sub->get_data(); - - ctrl_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); - ctrl = (struct actuator_controls_s *)ctrl_sub->get_data(); } void send(const hrt_abstime t) @@ -787,7 +781,6 @@ protected: bool updated = status_sub->update(t); updated |= pos_sp_triplet_sub->update(t); updated |= act_sub->update(t); - updated |= ctrl_sub->update(t); if (updated) { /* translate the current syste state to mavlink state and mode */ @@ -842,12 +835,30 @@ protected: 0); } else { - /* no special handling required, just send the controls */ + /* fixed wing: scale all channels except throttle -1 .. 1 + * because we know that we set the mixers up this way + */ + + float out[8]; + + const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + + for (unsigned i = 0; i < 8; i++) { + if (i != 3) { + /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ + out[i] = (act->output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + + } else { + + /* scale fake PWM out 900..2100 us to 0..1 for throttle */ + out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + } + + } mavlink_msg_hil_controls_send(_channel, hrt_absolute_time(), - ctrl->control[0], ctrl->control[1], ctrl->control[2], ctrl->control[3], - ctrl->control[4], ctrl->control[5], ctrl->control[6], ctrl->control[7], + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], mavlink_base_mode, 0); } -- cgit v1.2.3 From f17c0b133559dc440a7789caa45767b95de84e7b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 3 Apr 2014 21:15:47 +0200 Subject: mavlink: implemented multicasting between mavlink instances (two options: forwarding: forward received messages from self to other mavlink instances, passing: send out messages received from other mavlink intances over serial --- src/modules/mavlink/mavlink_main.cpp | 197 ++++++++++++++++++++++++++++++- src/modules/mavlink/mavlink_main.h | 34 +++++- src/modules/mavlink/mavlink_messages.cpp | 6 +- src/modules/mavlink/mavlink_receiver.cpp | 5 + 4 files changed, 236 insertions(+), 6 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 18df577fe..c5055939e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -207,10 +207,15 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), _mission_pub(-1), + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _uart_fd(-1), _mavlink_param_queue_index(0), _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _message_buffer({}), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) @@ -261,7 +266,6 @@ Mavlink::Mavlink() : errx(1, "instance ID is out of range"); break; } - } Mavlink::~Mavlink() @@ -394,6 +398,20 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) return false; } +void +Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) +{ + Mavlink *inst = ::_mavlink_instances; + + while (inst != nullptr) { + /* don't broadcast to itself */ + if (inst != self) { + inst->pass_message(msg); + } + inst = inst->next; + } +} + int Mavlink::get_uart_fd(unsigned index) { @@ -1616,6 +1634,125 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) } } +int +Mavlink::message_buffer_init(int size) +{ + _message_buffer.size = size; + _message_buffer.write_ptr = 0; + _message_buffer.read_ptr = 0; + _message_buffer.data = (char*)malloc(_message_buffer.size); + return (_message_buffer.data == 0) ? ERROR : OK; +} + +void +Mavlink::message_buffer_destroy() +{ + _message_buffer.size = 0; + _message_buffer.write_ptr = 0; + _message_buffer.read_ptr = 0; + free(_message_buffer.data); +} + +int +Mavlink::message_buffer_count() +{ + int n = _message_buffer.write_ptr - _message_buffer.read_ptr; + + if (n < 0) { + n += _message_buffer.size; + } + + return n; +} + +int +Mavlink::message_buffer_is_empty() +{ + return _message_buffer.read_ptr == _message_buffer.write_ptr; +} + + +bool +Mavlink::message_buffer_write(void *ptr, int size) +{ + // bytes available to write + int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; + + if (available < 0) { + available += _message_buffer.size; + } + + if (size > available) { + // buffer overflow + return false; + } + + char *c = (char *) ptr; + int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer + + if (n < size) { + // message goes over end of the buffer + memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n); + _message_buffer.write_ptr = 0; + + } else { + n = 0; + } + + // now: n = bytes already written + int p = size - n; // number of bytes to write + memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p); + _message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size; + return true; +} + +int +Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part) +{ + // bytes available to read + int available = _message_buffer.write_ptr - _message_buffer.read_ptr; + + if (available == 0) { + return 0; // buffer is empty + } + + int n = 0; + + if (available > 0) { + // read pointer is before write pointer, all available bytes can be read + n = available; + *is_part = false; + + } else { + // read pointer is after write pointer, read bytes from read_ptr to end of the buffer + n = _message_buffer.size - _message_buffer.read_ptr; + *is_part = _message_buffer.write_ptr > 0; + } + + *ptr = &(_message_buffer.data[_message_buffer.read_ptr]); + return n; +} + +void +Mavlink::message_buffer_mark_read(int n) +{ + _message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size; +} + +void +Mavlink::pass_message(mavlink_message_t *msg) +{ + if (_passing_on) { + /* size is 8 bytes plus variable payload */ + int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len; + pthread_mutex_lock(&_message_buffer_mutex); + message_buffer_write(msg, size); + pthread_mutex_unlock(&_message_buffer_mutex); + } +} + + + int Mavlink::task_main(int argc, char *argv[]) { @@ -1632,7 +1769,7 @@ Mavlink::task_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "b:r:d:m:v")) != EOF) { + while ((ch = getopt(argc, argv, "b:r:d:m:fpv")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1672,6 +1809,14 @@ Mavlink::task_main(int argc, char *argv[]) break; + case 'f': + _forwarding_on = true; + break; + + case 'p': + _passing_on = true; + break; + case 'v': _verbose = true; break; @@ -1740,6 +1885,17 @@ Mavlink::task_main(int argc, char *argv[]) /* initialize mavlink text message buffering */ mavlink_logbuffer_init(&_logbuffer, 5); + /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ + if (_passing_on) { + /* initialize message buffer if multiplexing is on */ + if (OK != message_buffer_init(500)) { + errx(1, "can't allocate message buffer, exiting"); + } + + /* initialize message buffer mutex */ + pthread_mutex_init(&_message_buffer_mutex, NULL); + } + /* create the device node that's used for sending text log messages, etc. */ register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); @@ -1884,6 +2040,37 @@ Mavlink::task_main(int argc, char *argv[]) } } + /* pass messages from other UARTs */ + if (_passing_on) { + + bool is_part; + void *read_ptr; + + /* guard get ptr by mutex */ + pthread_mutex_lock(&_message_buffer_mutex); + int available = message_buffer_get_ptr(&read_ptr, &is_part); + pthread_mutex_unlock(&_message_buffer_mutex); + + if (available > 0) { + /* write first part of buffer */ + _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + message_buffer_mark_read(available); + + /* write second part of buffer if there is some */ + if (is_part) { + /* guard get ptr by mutex */ + pthread_mutex_lock(&_message_buffer_mutex); + available = message_buffer_get_ptr(&read_ptr, &is_part); + pthread_mutex_unlock(&_message_buffer_mutex); + + _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + message_buffer_mark_read(available); + } + } + } + + + perf_end(_loop_perf); } @@ -1928,6 +2115,10 @@ Mavlink::task_main(int argc, char *argv[]) /* close mavlink logging device */ close(_mavlink_fd); + if (_passing_on) { + message_buffer_destroy(); + pthread_mutex_destroy(&_message_buffer_mutex); + } /* destroy log buffer */ mavlink_logbuffer_destroy(&_logbuffer); @@ -2067,7 +2258,7 @@ Mavlink::stream(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-v]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5a118a0ad..4f9a53a5b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -138,6 +138,8 @@ public: static bool instance_exists(const char *device_name, Mavlink *self); + static void forward_message(mavlink_message_t *msg, Mavlink *self); + static int get_uart_fd(unsigned index); int get_uart_fd(); @@ -153,10 +155,12 @@ public: void set_mode(enum MAVLINK_MODE); enum MAVLINK_MODE get_mode() { return _mode; } - bool get_hil_enabled() { return _hil_enabled; }; + bool get_hil_enabled() { return _hil_enabled; } bool get_flow_control_enabled() { return _flow_control_enabled; } + bool get_forwarding_on() { return _forwarding_on; } + /** * Handle waypoint related messages. */ @@ -234,6 +238,8 @@ private: mavlink_wpm_storage *_wpm; bool _verbose; + bool _forwarding_on; + bool _passing_on; int _uart_fd; int _baudrate; int _datarate; @@ -252,6 +258,16 @@ private: bool _flow_control_enabled; + struct mavlink_message_buffer { + int write_ptr; + int read_ptr; + int size; + char *data; + }; + mavlink_message_buffer _message_buffer; + + pthread_mutex_t _message_buffer_mutex; + /** * Send one parameter. * @@ -315,6 +331,22 @@ private: int configure_stream(const char *stream_name, const float rate); void configure_stream_threadsafe(const char *stream_name, const float rate); + int message_buffer_init(int size); + + void message_buffer_destroy(); + + int message_buffer_count(); + + int message_buffer_is_empty(); + + bool message_buffer_write(void *ptr, int size); + + int message_buffer_get_ptr(void **ptr, bool *is_part); + + void message_buffer_mark_read(int n); + + void pass_message(mavlink_message_t *msg); + static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); /** diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4ca3840d4..2b5d65080 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1265,11 +1265,13 @@ protected: || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ - mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + /* XXX TODO: get param for system ID */ + mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); } else { /* send camera capture off */ - mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + /* XXX TODO: get param for system ID */ + mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); } } }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d7e300670..1581f30d3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -913,6 +913,11 @@ MavlinkReceiver::receive_thread(void *arg) /* handle packet with parameter component */ _mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg); + + if (_mavlink->get_forwarding_on()) { + /* forward any messages to other mavlink instances */ + Mavlink::forward_message(&msg, _mavlink); + } } } } -- cgit v1.2.3 From de3efc0975f1b84cf556bae11bbebe95609dbcdc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 17:17:56 +0400 Subject: mavlink: publish SYS_STATUS at constant rate, don't look at update() result --- src/modules/mavlink/mavlink_messages.cpp | 31 +++++++++++++++---------------- 1 file changed, 15 insertions(+), 16 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4ca3840d4..c89031fcc 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -262,22 +262,21 @@ protected: void send(const hrt_abstime t) { - if (status_sub->update(t)) { - mavlink_msg_sys_status_send(_channel, - status->onboard_control_sensors_present, - status->onboard_control_sensors_enabled, - status->onboard_control_sensors_health, - status->load * 1000.0f, - status->battery_voltage * 1000.0f, - status->battery_current * 1000.0f, - status->battery_remaining, - status->drop_rate_comm, - status->errors_comm, - status->errors_count1, - status->errors_count2, - status->errors_count3, - status->errors_count4); - } + status_sub->update(t); + mavlink_msg_sys_status_send(_channel, + status->onboard_control_sensors_present, + status->onboard_control_sensors_enabled, + status->onboard_control_sensors_health, + status->load * 1000.0f, + status->battery_voltage * 1000.0f, + status->battery_current * 1000.0f, + status->battery_remaining, + status->drop_rate_comm, + status->errors_comm, + status->errors_count1, + status->errors_count2, + status->errors_count3, + status->errors_count4); } }; -- cgit v1.2.3 From 38c3e68976c8dc167b4d1e5d24792401fc7cc7d3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Apr 2014 21:44:01 +0200 Subject: Send camera command to all, use own sysid --- src/modules/mavlink/mavlink_messages.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2b5d65080..1e6bd6f93 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1265,13 +1265,11 @@ protected: || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ - /* XXX TODO: get param for system ID */ - mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); } else { /* send camera capture off */ - /* XXX TODO: get param for system ID */ - mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); } } }; -- cgit v1.2.3 From f0d043c06849a2f2f5e07d6f3bdfafc0e4e6f041 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 17:08:35 +0200 Subject: mavlink: COMMAND_LONG stream: listen to vehicle_command uorb topic and send mavlink_msg_command_long --- src/modules/mavlink/mavlink_messages.cpp | 46 ++++++++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 45b86a497..4d7b9160d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1273,6 +1273,51 @@ protected: } }; +class MavlinkStreamCommandLong : public MavlinkStream +{ +public: + const char *get_name() + { + return "COMMAND_LONG"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamCommandLong(); + } + +private: + MavlinkOrbSubscription *vehicle_command_sub; + struct vehicle_command_s *vehicle_command; + +protected: + void subscribe(Mavlink *mavlink) + { + vehicle_command_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); + vehicle_command = (struct vehicle_command_s *)vehicle_command_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (vehicle_command_sub->update(t)) { + if (!((vehicle_command->target_system == mavlink_system.sysid) && (vehicle_command->target_component == mavlink_system.compid))) { + mavlink_msg_command_long_send(_channel, + vehicle_command->target_system, + vehicle_command->target_component, + vehicle_command->command, + vehicle_command->confirmation, + vehicle_command->param1, + vehicle_command->param2, + vehicle_command->param3, + vehicle_command->param4, + vehicle_command->param5, + vehicle_command->param6, + vehicle_command->param7); + } + } + } +}; + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -1299,5 +1344,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), + new MavlinkStreamCommandLong(), nullptr }; -- cgit v1.2.3 From d55e64d1e54542762510387a22897f504c68a5a6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Apr 2014 23:29:40 +0400 Subject: mavlink: minor comments and formatting fixes --- src/modules/mavlink/mavlink_messages.cpp | 2 -- src/modules/mavlink/mavlink_stream.h | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 45b86a497..47603b390 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1252,8 +1252,6 @@ protected: { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); status = (struct vehicle_status_s *)status_sub->get_data(); - - } void send(const hrt_abstime t) diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 135e1bce0..def40d9ad 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file mavlink_stream.cpp + * @file mavlink_stream.h * Mavlink messages stream definition. * * @author Anton Babushkin -- cgit v1.2.3 From 27884e49bedfdbd0e259bb9b90e757b45bf7fd44 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Apr 2014 21:40:05 +0200 Subject: Revert "Merge pull request #816 from PX4/mavlink_commandlongstream" This reverts commit 00ef10f307d3c4a262a15ab5747d854eb4c568d5, reversing changes made to d55e64d1e54542762510387a22897f504c68a5a6. --- src/modules/mavlink/mavlink_messages.cpp | 46 -------------------------------- 1 file changed, 46 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index bb8bba037..47603b390 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1271,51 +1271,6 @@ protected: } }; -class MavlinkStreamCommandLong : public MavlinkStream -{ -public: - const char *get_name() - { - return "COMMAND_LONG"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamCommandLong(); - } - -private: - MavlinkOrbSubscription *vehicle_command_sub; - struct vehicle_command_s *vehicle_command; - -protected: - void subscribe(Mavlink *mavlink) - { - vehicle_command_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); - vehicle_command = (struct vehicle_command_s *)vehicle_command_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (vehicle_command_sub->update(t)) { - if (!((vehicle_command->target_system == mavlink_system.sysid) && (vehicle_command->target_component == mavlink_system.compid))) { - mavlink_msg_command_long_send(_channel, - vehicle_command->target_system, - vehicle_command->target_component, - vehicle_command->command, - vehicle_command->confirmation, - vehicle_command->param1, - vehicle_command->param2, - vehicle_command->param3, - vehicle_command->param4, - vehicle_command->param5, - vehicle_command->param6, - vehicle_command->param7); - } - } - } -}; - MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -1342,6 +1297,5 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), - new MavlinkStreamCommandLong(), nullptr }; -- cgit v1.2.3 From e99291d825cfc89c67cadd8487e568d3b21d218f Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 09:08:37 +0200 Subject: Added vicon stream. --- src/modules/mavlink/mavlink_messages.cpp | 42 ++++++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 47603b390..37929edac 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -640,6 +640,47 @@ protected: }; + +class MavlinkStreamViconPositionEstimate : public MavlinkStream +{ +public: + const char *get_name() + { + return "VICON_POSITION_ESTIMATE"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamViconPositionEstimate(); + } + +private: + MavlinkOrbSubscription *pos_sub; + struct vehicle_vicon_position_s *pos; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); + pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (pos_sub->update(t)) { + mavlink_msg_vicon_position_estimate_send(_channel, + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->roll, + pos->pitch, + pos->yaw); + } + } +}; + + class MavlinkStreamGPSGlobalOrigin : public MavlinkStream { public: @@ -1297,5 +1338,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), + new MavlinkStreamViconPositionEstimate(), nullptr }; -- cgit v1.2.3 From 8a946f0320c4bd4a61927a12b7ba4c0c96c77d7d Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:37:58 +0200 Subject: More whitespace all the time. --- src/modules/mavlink/mavlink_messages.cpp | 180 +++++++++++++++---------------- src/modules/mavlink/mavlink_receiver.cpp | 6 +- 2 files changed, 93 insertions(+), 93 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 37929edac..2d1d92243 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -94,7 +94,7 @@ cm_uint16_from_m_float(float m) } void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, - uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { *mavlink_state = 0; *mavlink_base_mode = 0; @@ -107,7 +107,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* arming state */ if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -163,8 +163,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* set system state */ if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; } else if (status->arming_state == ARMING_STATE_ARMED) { @@ -344,13 +344,13 @@ protected: } mavlink_msg_highres_imu_send(_channel, - sensor->timestamp, - sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], - sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], - sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], - sensor->baro_pres_mbar, sensor->differential_pressure_pa, - sensor->baro_alt_meter, sensor->baro_temp_celcius, - fields_updated); + sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); } } }; @@ -420,14 +420,14 @@ protected: { if (att_sub->update(t)) { mavlink_msg_attitude_quaternion_send(_channel, - att->timestamp / 1000, - att->q[0], - att->q[1], - att->q[2], - att->q[3], - att->rollspeed, - att->pitchspeed, - att->yawspeed); + att->timestamp / 1000, + att->q[0], + att->q[1], + att->q[2], + att->q[3], + att->rollspeed, + att->pitchspeed, + att->yawspeed); } } }; @@ -534,16 +534,16 @@ protected: { if (gps_sub->update(t)) { mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph_m), - cm_uint16_from_m_float(gps->epv_m), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); + gps->timestamp_position, + gps->fix_type, + gps->lat, + gps->lon, + gps->alt, + cm_uint16_from_m_float(gps->eph_m), + cm_uint16_from_m_float(gps->epv_m), + gps->vel_m_s * 100.0f, + _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps->satellites_visible); } } }; @@ -586,15 +586,15 @@ protected: if (updated) { mavlink_msg_global_position_int_send(_channel, - pos->timestamp / 1000, - pos->lat * 1e7, - pos->lon * 1e7, - pos->alt * 1000.0f, - (pos->alt - home->alt) * 1000.0f, - pos->vel_n * 100.0f, - pos->vel_e * 100.0f, - pos->vel_d * 100.0f, - _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + pos->timestamp / 1000, + pos->lat * 1e7, + pos->lon * 1e7, + pos->alt * 1000.0f, + (pos->alt - home->alt) * 1000.0f, + pos->vel_n * 100.0f, + pos->vel_e * 100.0f, + pos->vel_d * 100.0f, + _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); } } }; @@ -628,13 +628,13 @@ protected: { if (pos_sub->update(t)) { mavlink_msg_local_position_ned_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->vx, - pos->vy, - pos->vz); + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->vx, + pos->vy, + pos->vz); } } }; @@ -644,40 +644,40 @@ protected: class MavlinkStreamViconPositionEstimate : public MavlinkStream { public: - const char *get_name() - { - return "VICON_POSITION_ESTIMATE"; - } + const char *get_name() + { + return "VICON_POSITION_ESTIMATE"; + } - MavlinkStream *new_instance() - { - return new MavlinkStreamViconPositionEstimate(); - } + MavlinkStream *new_instance() + { + return new MavlinkStreamViconPositionEstimate(); + } private: - MavlinkOrbSubscription *pos_sub; - struct vehicle_vicon_position_s *pos; + MavlinkOrbSubscription *pos_sub; + struct vehicle_vicon_position_s *pos; protected: - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); - pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (pos_sub->update(t)) { - mavlink_msg_vicon_position_estimate_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->roll, - pos->pitch, - pos->yaw); - } - } + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); + pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (pos_sub->update(t)) { + mavlink_msg_vicon_position_estimate_send(_channel, + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->roll, + pos->pitch, + pos->yaw); + } + } }; @@ -869,10 +869,10 @@ protected: } mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } else { /* fixed wing: scale all channels except throttle -1 .. 1 @@ -897,10 +897,10 @@ protected: } mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } } } @@ -1175,12 +1175,12 @@ protected: { if (flow_sub->update(t)) { mavlink_msg_optical_flow_send(_channel, - flow->timestamp, - flow->sensor_id, - flow->flow_raw_x, flow->flow_raw_y, - flow->flow_comp_x_m, flow->flow_comp_y_m, - flow->quality, - flow->ground_distance_m); + flow->timestamp, + flow->sensor_id, + flow->flow_raw_x, flow->flow_raw_y, + flow->flow_comp_x_m, flow->flow_comp_y_m, + flow->quality, + flow->ground_distance_m); } } }; @@ -1300,7 +1300,7 @@ protected: (void)status_sub->update(t); if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); @@ -1338,6 +1338,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), - new MavlinkStreamViconPositionEstimate(), + new MavlinkStreamViconPositionEstimate(), nullptr }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 61ef2b043..b4fe65fd2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -182,9 +182,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } } - /* If we've received a valid message, mark the flag indicating so. - This is used in the '-w' command-line flag. */ - _mavlink->set_has_received_messages(true); + /* If we've received a valid message, mark the flag indicating so. + This is used in the '-w' command-line flag. */ + _mavlink->set_has_received_messages(true); } void -- cgit v1.2.3 From e7c8fdc586e59d50579470336157feb14c65ac5b Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:41:00 +0200 Subject: More whitespace all the time. --- src/modules/mavlink/mavlink_messages.cpp | 134 +++++++++++++++---------------- 1 file changed, 67 insertions(+), 67 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2d1d92243..648228e3b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -94,7 +94,7 @@ cm_uint16_from_m_float(float m) } void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, - uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { *mavlink_state = 0; *mavlink_base_mode = 0; @@ -107,7 +107,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* arming state */ if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -163,8 +163,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* set system state */ if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; } else if (status->arming_state == ARMING_STATE_ARMED) { @@ -344,13 +344,13 @@ protected: } mavlink_msg_highres_imu_send(_channel, - sensor->timestamp, - sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], - sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], - sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], - sensor->baro_pres_mbar, sensor->differential_pressure_pa, - sensor->baro_alt_meter, sensor->baro_temp_celcius, - fields_updated); + sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); } } }; @@ -420,14 +420,14 @@ protected: { if (att_sub->update(t)) { mavlink_msg_attitude_quaternion_send(_channel, - att->timestamp / 1000, - att->q[0], - att->q[1], - att->q[2], - att->q[3], - att->rollspeed, - att->pitchspeed, - att->yawspeed); + att->timestamp / 1000, + att->q[0], + att->q[1], + att->q[2], + att->q[3], + att->rollspeed, + att->pitchspeed, + att->yawspeed); } } }; @@ -534,16 +534,16 @@ protected: { if (gps_sub->update(t)) { mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph_m), - cm_uint16_from_m_float(gps->epv_m), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); + gps->timestamp_position, + gps->fix_type, + gps->lat, + gps->lon, + gps->alt, + cm_uint16_from_m_float(gps->eph_m), + cm_uint16_from_m_float(gps->epv_m), + gps->vel_m_s * 100.0f, + _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps->satellites_visible); } } }; @@ -586,15 +586,15 @@ protected: if (updated) { mavlink_msg_global_position_int_send(_channel, - pos->timestamp / 1000, - pos->lat * 1e7, - pos->lon * 1e7, - pos->alt * 1000.0f, - (pos->alt - home->alt) * 1000.0f, - pos->vel_n * 100.0f, - pos->vel_e * 100.0f, - pos->vel_d * 100.0f, - _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + pos->timestamp / 1000, + pos->lat * 1e7, + pos->lon * 1e7, + pos->alt * 1000.0f, + (pos->alt - home->alt) * 1000.0f, + pos->vel_n * 100.0f, + pos->vel_e * 100.0f, + pos->vel_d * 100.0f, + _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); } } }; @@ -628,13 +628,13 @@ protected: { if (pos_sub->update(t)) { mavlink_msg_local_position_ned_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->vx, - pos->vy, - pos->vz); + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->vx, + pos->vy, + pos->vz); } } }; @@ -669,13 +669,13 @@ protected: { if (pos_sub->update(t)) { mavlink_msg_vicon_position_estimate_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->roll, - pos->pitch, - pos->yaw); + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->roll, + pos->pitch, + pos->yaw); } } }; @@ -869,10 +869,10 @@ protected: } mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } else { /* fixed wing: scale all channels except throttle -1 .. 1 @@ -897,10 +897,10 @@ protected: } mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } } } @@ -1175,12 +1175,12 @@ protected: { if (flow_sub->update(t)) { mavlink_msg_optical_flow_send(_channel, - flow->timestamp, - flow->sensor_id, - flow->flow_raw_x, flow->flow_raw_y, - flow->flow_comp_x_m, flow->flow_comp_y_m, - flow->quality, - flow->ground_distance_m); + flow->timestamp, + flow->sensor_id, + flow->flow_raw_x, flow->flow_raw_y, + flow->flow_comp_x_m, flow->flow_comp_y_m, + flow->quality, + flow->ground_distance_m); } } }; @@ -1300,7 +1300,7 @@ protected: (void)status_sub->update(t); if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); -- cgit v1.2.3 From f84669039579aeef80e232c3c9a444d20bcbdf39 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Apr 2014 23:30:09 +0200 Subject: Added rangefinder message to MAVLink app --- src/modules/mavlink/mavlink_main.cpp | 1 + src/modules/mavlink/mavlink_messages.cpp | 47 ++++++++++++++++++++++++++++++++ src/modules/mavlink/module.mk | 2 ++ 3 files changed, 50 insertions(+) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1ed3f4001..c90b4e2de 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1942,6 +1942,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult); configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult); + configure_stream("DISTANCE_SENSOR", 0.5f); break; case MAVLINK_MODE_CAMERA: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 47603b390..e608bf787 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include "mavlink_messages.h" @@ -1271,6 +1272,51 @@ protected: } }; +class MavlinkStreamDistanceSensor : public MavlinkStream +{ +public: + const char *get_name() + { + return "DISTANCE_SENSOR"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamDistanceSensor(); + } + +private: + MavlinkOrbSubscription *range_sub; + struct range_finder_report *range; + +protected: + void subscribe(Mavlink *mavlink) + { + range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); + range = (struct range_finder_report *)range_sub->get_data(); + } + + void send(const hrt_abstime t) + { + (void)range_sub->update(t); + + uint8_t type; + + switch (range->type) { + case RANGE_FINDER_TYPE_LASER: + type = MAV_DISTANCE_SENSOR_LASER; + break; + } + + uint8_t id = 0; + uint8_t orientation = 0; + uint8_t covariance = 20; + + mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, + range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); + } +}; + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -1297,5 +1343,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), + new MavlinkStreamDistanceSensor(), nullptr }; diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 515fbfadc..dcca11977 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -46,3 +46,5 @@ SRCS += mavlink_main.cpp \ mavlink_commands.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 1f2e972ea683a4d2f33af0e91308f6efed2465c9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 21 Apr 2014 12:16:45 +0200 Subject: mavlink: remaining battery scaling fixed --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 58af1fcb6..678ce1645 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -271,7 +271,7 @@ protected: status->load * 1000.0f, status->battery_voltage * 1000.0f, status->battery_current * 1000.0f, - status->battery_remaining, + status->battery_remaining * 100.0f, status->drop_rate_comm, status->errors_comm, status->errors_count1, -- cgit v1.2.3 From 269800b48c31d78fec900b4beaf3f655a8c18730 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Sun, 27 Apr 2014 14:06:00 -0700 Subject: renamed EASY to POSHOLD and SEATBELT to ALTHOLD --- src/drivers/blinkm/blinkm.cpp | 4 +- src/modules/commander/commander.cpp | 46 +++++++++++----------- .../commander_tests/state_machine_helper_test.cpp | 24 +++++------ src/modules/commander/px4_custom_mode.h | 4 +- src/modules/commander/state_machine_helper.cpp | 4 +- src/modules/fixedwing_backside/fixedwing.cpp | 4 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 42 ++++++++++---------- src/modules/mavlink/mavlink_messages.cpp | 8 ++-- src/modules/mc_pos_control/mc_pos_control_params.c | 10 ++--- src/modules/segway/BlockSegwayController.cpp | 4 +- src/modules/sensors/sensor_params.c | 6 +-- src/modules/sensors/sensors.cpp | 26 ++++++------ src/modules/uORB/topics/manual_control_setpoint.h | 4 +- src/modules/uORB/topics/rc_channels.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 4 +- 15 files changed, 96 insertions(+), 96 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index b75c2297f..aa9c64502 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -638,11 +638,11 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_EASY) + if (vehicle_status_raw.main_state == MAIN_STATE_POSHOLD) led_color_4 = LED_GREEN; else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_SEATBELT) + else if (vehicle_status_raw.main_state == MAIN_STATE_ALTHOLD) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 50380107d..a99456370 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -434,13 +434,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTHOLD) { + /* ALTHOLD */ + main_res = main_state_transition(status, MAIN_STATE_ALTHOLD); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSHOLD) { + /* POSHOLD */ + main_res = main_state_transition(status, MAIN_STATE_POSHOLD); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -455,8 +455,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); + /* POSHOLD */ + main_res = main_state_transition(status, MAIN_STATE_POSHOLD); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -628,8 +628,8 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "SEATBELT"; - main_states_str[2] = "EASY"; + main_states_str[1] = "ALTHOLD"; + main_states_str[2] = "POSHOLD"; main_states_str[3] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -1299,8 +1299,8 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.easy_switch == SWITCH_POS_ON) { + /* flight termination in manual mode if assisted switch is on poshold position */ + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.poshold_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1557,25 +1557,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->easy_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_EASY); + if (sp_man->poshold_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_POSHOLD); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to SEATBELT - print_reject_mode(status, "EASY"); + // else fallback to ALTHOLD + print_reject_mode(status, "POSHOLD"); } - res = main_state_transition(status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_ALTHOLD); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->easy_switch != SWITCH_POS_ON) { - print_reject_mode(status, "SEATBELT"); + if (sp_man->poshold_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTHOLD"); } // else fallback to MANUAL @@ -1590,9 +1590,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to SEATBELT (EASY likely will not work too) + // else fallback to ALTHOLD (POSHOLD likely will not work too) print_reject_mode(status, "AUTO"); - res = main_state_transition(status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_ALTHOLD); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1638,7 +1638,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_SEATBELT: + case MAIN_STATE_ALTHOLD: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -1649,7 +1649,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_EASY: + case MAIN_STATE_POSHOLD: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 8a946543d..b440e561b 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to SEATBELT. + // MANUAL to ALTHOLD. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_SEATBELT; - ut_assert("tranisition: manual to seatbelt", + new_main_state = MAIN_STATE_ALTHOLD; + ut_assert("tranisition: manual to althold", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); + ut_assert("new state: althold", MAIN_STATE_ALTHOLD == current_state.main_state); - // MANUAL to SEATBELT, invalid local altitude. + // MANUAL to ALTHOLD, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_SEATBELT; + new_main_state = MAIN_STATE_ALTHOLD; ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to EASY. + // MANUAL to POSHOLD. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_EASY; - ut_assert("transition: manual to easy", + new_main_state = MAIN_STATE_POSHOLD; + ut_assert("transition: manual to poshold", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); + ut_assert("current state: poshold", MAIN_STATE_POSHOLD == current_state.main_state); - // MANUAL to EASY, invalid local position. + // MANUAL to POSHOLD, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_EASY; + new_main_state = MAIN_STATE_POSHOLD; ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 2144d3460..962d2804c 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, - PX4_CUSTOM_MAIN_MODE_SEATBELT, - PX4_CUSTOM_MAIN_MODE_EASY, + PX4_CUSTOM_MAIN_MODE_ALTHOLD, + PX4_CUSTOM_MAIN_MODE_POSHOLD, PX4_CUSTOM_MAIN_MODE_AUTO, }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f09d586c7..21e36a87d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -215,7 +215,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; - case MAIN_STATE_SEATBELT: + case MAIN_STATE_ALTHOLD: /* need at minimum altitude estimate */ if (!status->is_rotary_wing || @@ -226,7 +226,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_EASY: + case MAIN_STATE_POSHOLD: /* need at minimum local position estimate */ if (status->condition_local_position_valid || diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index cfae07275..fafab9bfe 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.main_state == MAIN_STATE_SEATBELT || - _status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) { + } else if (_status.main_state == MAIN_STATE_ALTHOLD || + _status.main_state == MAIN_STATE_POSHOLD /* TODO, implement easy */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between 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 7f13df785..d5a68e21f 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 @@ -153,7 +153,7 @@ private: bool _setpoint_valid; /**< flag if the position control setpoint is valid */ /** manual control states */ - float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ + float _althold_hold_heading; /**< heading the system should hold in althold mode */ double _loiter_hold_lat; double _loiter_hold_lon; float _loiter_hold_alt; @@ -1051,16 +1051,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* easy mode enabled */) { + } else if (0/* poshold mode enabled */) { - /** EASY FLIGHT **/ + /** POSHOLD FLIGHT **/ - if (0/* switched from another mode to easy */) { - _seatbelt_hold_heading = _att.yaw; + if (0/* switched from another mode to poshold */) { + _althold_hold_heading = _att.yaw; } - if (0/* easy on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; + if (0/* poshold on and manual control yaw non-zero */) { + _althold_hold_heading = _att.yaw + _manual.yaw; } //XXX not used @@ -1073,39 +1073,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // climb_out = true; // } - /* if in seatbelt mode, set airspeed based on manual control */ + /* if in althold mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float seatbelt_airspeed = _parameters.airspeed_min + + float althold_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, + althold_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, false, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); - } else if (0/* seatbelt mode enabled */) { + } else if (0/* althold mode enabled */) { - /** SEATBELT FLIGHT **/ + /** ALTHOLD FLIGHT **/ - if (0/* switched from another mode to seatbelt */) { - _seatbelt_hold_heading = _att.yaw; + if (0/* switched from another mode to althold */) { + _althold_hold_heading = _att.yaw; } - if (0/* seatbelt on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; + if (0/* althold on and manual control yaw non-zero */) { + _althold_hold_heading = _att.yaw + _manual.yaw; } - /* if in seatbelt mode, set airspeed based on manual control */ + /* if in althold mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float seatbelt_airspeed = _parameters.airspeed_min + + float althold_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; @@ -1124,11 +1124,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi climb_out = true; } - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _manual.roll; _att_sp.yaw_body = _manual.yaw; _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, + althold_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 678ce1645..108ef8ad4 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_SEATBELT) { + } else if (status->main_state == MAIN_STATE_ALTHOLD) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTHOLD; - } else if (status->main_state == MAIN_STATE_EASY) { + } else if (status->main_state == MAIN_STATE_POSHOLD) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSHOLD; } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 104df4e59..015d8ad16 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); /** * Maximum vertical velocity * - * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY). + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTHOLD, POSHOLD). * * @unit m/s * @min 0.0 @@ -109,7 +109,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); /** * Vertical velocity feed forward * - * Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for altitude control in stabilized modes (ALTHOLD, POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -154,7 +154,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); /** * Maximum horizontal velocity * - * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY). + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSHOLD). * * @unit m/s * @min 0.0 @@ -165,7 +165,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); /** * Horizontal velocity feed forward * - * Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for position control in position control mode (POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -176,7 +176,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); /** * Maximum tilt angle in air * - * Limits maximum tilt in AUTO and EASY modes during flight. + * Limits maximum tilt in AUTO and POSHOLD modes during flight. * * @unit deg * @min 0.0 diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 96a443c6e..2cb4fc900 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_SEATBELT || - _status.main_state == MAIN_STATE_EASY) { + _status.main_state == MAIN_STATE_ALTHOLD || + _status.main_state == MAIN_STATE_POSHOLD) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index dd6e21bdd..59bd99db7 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -605,7 +605,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_EASY_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_POSHLD_SW, 0); /** * Loiter switch channel mapping. @@ -703,7 +703,7 @@ PARAM_DEFINE_FLOAT(RC_ASSISTED_TH, 0.25f); PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); /** - * Threshold for selecting easy mode + * Threshold for selecting poshold mode * * min:-1 * max:+1 @@ -716,7 +716,7 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); * negative : true when channel Date: Mon, 28 Apr 2014 21:47:45 -0700 Subject: Replaces poshold/althold with posctrl/altctrl --- mavlink/include/mavlink/v1.0/autoquad/autoquad.h | 4 +- src/drivers/blinkm/blinkm.cpp | 4 +- src/modules/commander/commander.cpp | 46 +++++++++++----------- .../commander_tests/state_machine_helper_test.cpp | 24 +++++------ src/modules/commander/px4_custom_mode.h | 4 +- src/modules/commander/state_machine_helper.cpp | 4 +- src/modules/fixedwing_backside/fixedwing.cpp | 4 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 42 ++++++++++---------- src/modules/mavlink/mavlink_messages.cpp | 8 ++-- src/modules/mc_pos_control/mc_pos_control_params.c | 10 ++--- src/modules/segway/BlockSegwayController.cpp | 4 +- src/modules/sensors/sensor_params.c | 6 +-- src/modules/sensors/sensors.cpp | 26 ++++++------ src/modules/uORB/topics/manual_control_setpoint.h | 2 +- src/modules/uORB/topics/rc_channels.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 4 +- 16 files changed, 97 insertions(+), 97 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h index 93e868dc3..bd3fc66e7 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h +++ b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h @@ -40,8 +40,8 @@ enum AUTOQUAD_NAV_STATUS AQ_NAV_STATUS_INIT=0, /* System is initializing | */ AQ_NAV_STATUS_STANDBY=1, /* System is standing by, not active | */ AQ_NAV_STATUS_MANUAL=2, /* Stabilized, under full manual control | */ - AQ_NAV_STATUS_ALTHOLD=4, /* Altitude hold engaged | */ - AQ_NAV_STATUS_POSHOLD=8, /* Position hold engaged | */ + AQ_NAV_STATUS_ALTCTRL=4, /* Altitude hold engaged | */ + AQ_NAV_STATUS_POSCTRL=8, /* Position hold engaged | */ AQ_NAV_STATUS_DVH=16, /* Dynamic Velocity Hold is active | */ AQ_NAV_STATUS_MISSION=32, /* Autonomous mission execution mode | */ AQ_NAV_STATUS_CEILING_REACHED=67108864, /* Craft is at ceiling altitude | */ diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index aa9c64502..974e20ca2 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -638,11 +638,11 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_POSHOLD) + if (vehicle_status_raw.main_state == MAIN_STATE_POSCTRL) led_color_4 = LED_GREEN; else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_ALTHOLD) + else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTRL) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a99456370..be3e6d269 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -434,13 +434,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTHOLD) { - /* ALTHOLD */ - main_res = main_state_transition(status, MAIN_STATE_ALTHOLD); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTRL) { + /* ALTCTRL */ + main_res = main_state_transition(status, MAIN_STATE_ALTCTRL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSHOLD) { - /* POSHOLD */ - main_res = main_state_transition(status, MAIN_STATE_POSHOLD); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTRL) { + /* POSCTRL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTRL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -455,8 +455,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* POSHOLD */ - main_res = main_state_transition(status, MAIN_STATE_POSHOLD); + /* POSCTRL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTRL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -628,8 +628,8 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "ALTHOLD"; - main_states_str[2] = "POSHOLD"; + main_states_str[1] = "ALTCTRL"; + main_states_str[2] = "POSCTRL"; main_states_str[3] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -1299,8 +1299,8 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assisted switch is on poshold position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.poshold_switch == SWITCH_POS_ON) { + /* flight termination in manual mode if assisted switch is on posctrl position */ + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctrl_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1557,25 +1557,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->poshold_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_POSHOLD); + if (sp_man->posctrl_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_POSCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to ALTHOLD - print_reject_mode(status, "POSHOLD"); + // else fallback to ALTCTRL + print_reject_mode(status, "POSCTRL"); } - res = main_state_transition(status, MAIN_STATE_ALTHOLD); + res = main_state_transition(status, MAIN_STATE_ALTCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->poshold_switch != SWITCH_POS_ON) { - print_reject_mode(status, "ALTHOLD"); + if (sp_man->posctrl_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTCTRL"); } // else fallback to MANUAL @@ -1590,9 +1590,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to ALTHOLD (POSHOLD likely will not work too) + // else fallback to ALTCTRL (POSCTRL likely will not work too) print_reject_mode(status, "AUTO"); - res = main_state_transition(status, MAIN_STATE_ALTHOLD); + res = main_state_transition(status, MAIN_STATE_ALTCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1638,7 +1638,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_ALTHOLD: + case MAIN_STATE_ALTCTRL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -1649,7 +1649,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_POSHOLD: + case MAIN_STATE_POSCTRL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index b440e561b..18586053b 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to ALTHOLD. + // MANUAL to ALTCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_ALTHOLD; - ut_assert("tranisition: manual to althold", + new_main_state = MAIN_STATE_ALTCTRL; + ut_assert("tranisition: manual to altctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: althold", MAIN_STATE_ALTHOLD == current_state.main_state); + ut_assert("new state: altctrl", MAIN_STATE_ALTCTRL == current_state.main_state); - // MANUAL to ALTHOLD, invalid local altitude. + // MANUAL to ALTCTRL, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_ALTHOLD; + new_main_state = MAIN_STATE_ALTCTRL; ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to POSHOLD. + // MANUAL to POSCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_POSHOLD; - ut_assert("transition: manual to poshold", + new_main_state = MAIN_STATE_POSCTRL; + ut_assert("transition: manual to posctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: poshold", MAIN_STATE_POSHOLD == current_state.main_state); + ut_assert("current state: posctrl", MAIN_STATE_POSCTRL == current_state.main_state); - // MANUAL to POSHOLD, invalid local position. + // MANUAL to POSCTRL, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_POSHOLD; + new_main_state = MAIN_STATE_POSCTRL; ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 962d2804c..e6274fb89 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, - PX4_CUSTOM_MAIN_MODE_ALTHOLD, - PX4_CUSTOM_MAIN_MODE_POSHOLD, + PX4_CUSTOM_MAIN_MODE_ALTCTRL, + PX4_CUSTOM_MAIN_MODE_POSCTRL, PX4_CUSTOM_MAIN_MODE_AUTO, }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 21e36a87d..3b6d95135 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -215,7 +215,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; - case MAIN_STATE_ALTHOLD: + case MAIN_STATE_ALTCTRL: /* need at minimum altitude estimate */ if (!status->is_rotary_wing || @@ -226,7 +226,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_POSHOLD: + case MAIN_STATE_POSCTRL: /* need at minimum local position estimate */ if (status->condition_local_position_valid || diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index fafab9bfe..dc82ee475 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.main_state == MAIN_STATE_ALTHOLD || - _status.main_state == MAIN_STATE_POSHOLD /* TODO, implement easy */) { + } else if (_status.main_state == MAIN_STATE_ALTCTRL || + _status.main_state == MAIN_STATE_POSCTRL /* TODO, implement easy */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between 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 d5a68e21f..024dd98ec 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 @@ -153,7 +153,7 @@ private: bool _setpoint_valid; /**< flag if the position control setpoint is valid */ /** manual control states */ - float _althold_hold_heading; /**< heading the system should hold in althold mode */ + float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */ double _loiter_hold_lat; double _loiter_hold_lon; float _loiter_hold_alt; @@ -1051,16 +1051,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* poshold mode enabled */) { + } else if (0/* posctrl mode enabled */) { - /** POSHOLD FLIGHT **/ + /** POSCTRL FLIGHT **/ - if (0/* switched from another mode to poshold */) { - _althold_hold_heading = _att.yaw; + if (0/* switched from another mode to posctrl */) { + _altctrl_hold_heading = _att.yaw; } - if (0/* poshold on and manual control yaw non-zero */) { - _althold_hold_heading = _att.yaw + _manual.yaw; + if (0/* posctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.yaw; } //XXX not used @@ -1073,39 +1073,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // climb_out = true; // } - /* if in althold mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float althold_airspeed = _parameters.airspeed_min + + float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; - _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - althold_airspeed, + altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, false, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); - } else if (0/* althold mode enabled */) { + } else if (0/* altctrl mode enabled */) { - /** ALTHOLD FLIGHT **/ + /** ALTCTRL FLIGHT **/ - if (0/* switched from another mode to althold */) { - _althold_hold_heading = _att.yaw; + if (0/* switched from another mode to altctrl */) { + _altctrl_hold_heading = _att.yaw; } - if (0/* althold on and manual control yaw non-zero */) { - _althold_hold_heading = _att.yaw + _manual.yaw; + if (0/* altctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.yaw; } - /* if in althold mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float althold_airspeed = _parameters.airspeed_min + + float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; @@ -1124,11 +1124,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi climb_out = true; } - _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _manual.roll; _att_sp.yaw_body = _manual.yaw; _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - althold_airspeed, + altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 108ef8ad4..38a5433ff 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_ALTHOLD) { + } else if (status->main_state == MAIN_STATE_ALTCTRL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTHOLD; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTRL; - } else if (status->main_state == MAIN_STATE_POSHOLD) { + } else if (status->main_state == MAIN_STATE_POSCTRL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSHOLD; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTRL; } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 015d8ad16..dacdd46f0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); /** * Maximum vertical velocity * - * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTHOLD, POSHOLD). + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL). * * @unit m/s * @min 0.0 @@ -109,7 +109,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); /** * Vertical velocity feed forward * - * Feed forward weight for altitude control in stabilized modes (ALTHOLD, POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -154,7 +154,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); /** * Maximum horizontal velocity * - * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSHOLD). + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL). * * @unit m/s * @min 0.0 @@ -165,7 +165,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); /** * Horizontal velocity feed forward * - * Feed forward weight for position control in position control mode (POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -176,7 +176,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); /** * Maximum tilt angle in air * - * Limits maximum tilt in AUTO and POSHOLD modes during flight. + * Limits maximum tilt in AUTO and POSCTRL modes during flight. * * @unit deg * @min 0.0 diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 2cb4fc900..6d46db9bd 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_ALTHOLD || - _status.main_state == MAIN_STATE_POSHOLD) { + _status.main_state == MAIN_STATE_ALTCTRL || + _status.main_state == MAIN_STATE_POSCTRL) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 59bd99db7..02890b452 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -605,7 +605,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_POSHLD_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); /** * Loiter switch channel mapping. @@ -703,7 +703,7 @@ PARAM_DEFINE_FLOAT(RC_ASSISTED_TH, 0.25f); PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); /** - * Threshold for selecting poshold mode + * Threshold for selecting posctrl mode * * min:-1 * max:+1 @@ -716,7 +716,7 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); * negative : true when channel Date: Sun, 4 May 2014 14:06:38 +0200 Subject: mavlink: Only sending HIL control commands if the system is actually armed --- src/modules/mavlink/mavlink_messages.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 678ce1645..b538aa28b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -819,11 +819,11 @@ protected: void send(const hrt_abstime t) { - bool updated = status_sub->update(t); - updated |= pos_sp_triplet_sub->update(t); - updated |= act_sub->update(t); + bool updated = act_sub->update(t); + (void)pos_sp_triplet_sub->update(t); + (void)status_sub->update(t); - if (updated) { + if (updated && (status.arming_state == ARMING_STATE_ARMED)) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state; uint8_t mavlink_base_mode; -- cgit v1.2.3 From 0e31b5935ebefef3b95d7e39dbf5b1435a4942a1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 4 May 2014 15:01:07 +0200 Subject: remove trailing whitespace --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b538aa28b..ab733e442 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1353,7 +1353,7 @@ protected: uint8_t orientation = 0; uint8_t covariance = 20; - mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, + mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); } }; -- cgit v1.2.3 From 5f786af8fa428c8ab684251310de6c3fbe795e8f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 4 May 2014 15:02:00 +0200 Subject: mavlink: status is a pointer --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index ab733e442..bef8a5a55 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -823,7 +823,7 @@ protected: (void)pos_sp_triplet_sub->update(t); (void)status_sub->update(t); - if (updated && (status.arming_state == ARMING_STATE_ARMED)) { + if (updated && (status->arming_state == ARMING_STATE_ARMED)) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state; uint8_t mavlink_base_mode; -- cgit v1.2.3 From df6a0d5a1a4f528e5ba22747d7a4587b7a2263c4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 6 May 2014 12:55:39 +0200 Subject: mavlink: Only send the distance sensor message if the topic actually updates --- src/modules/mavlink/mavlink_messages.cpp | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index bef8a5a55..9c552515d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1339,22 +1339,23 @@ protected: void send(const hrt_abstime t) { - (void)range_sub->update(t); + if (range_sub->update(t)) { - uint8_t type; + uint8_t type; - switch (range->type) { - case RANGE_FINDER_TYPE_LASER: - type = MAV_DISTANCE_SENSOR_LASER; - break; - } + switch (range->type) { + case RANGE_FINDER_TYPE_LASER: + type = MAV_DISTANCE_SENSOR_LASER; + break; + } - uint8_t id = 0; - uint8_t orientation = 0; - uint8_t covariance = 20; + uint8_t id = 0; + uint8_t orientation = 0; + uint8_t covariance = 20; - mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, - range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); + mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, + range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); + } } }; -- cgit v1.2.3 From 88b18bbad1be31cf31ff964c6cf6f3123948488d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 11 May 2014 13:35:05 +0200 Subject: ALTCTRL/POSCTRL renamed to ALTCTL/POSCTL --- src/drivers/blinkm/blinkm.cpp | 4 +- src/modules/commander/commander.cpp | 46 +++++++++++----------- .../commander_tests/state_machine_helper_test.cpp | 12 +++--- src/modules/commander/px4_custom_mode.h | 4 +- src/modules/commander/state_machine_helper.cpp | 4 +- src/modules/fixedwing_backside/fixedwing.cpp | 4 +- src/modules/mavlink/mavlink_messages.cpp | 8 ++-- src/modules/segway/BlockSegwayController.cpp | 4 +- src/modules/uORB/topics/manual_control_setpoint.h | 4 +- src/modules/uORB/topics/vehicle_status.h | 4 +- 10 files changed, 47 insertions(+), 47 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 974e20ca2..c41d8f242 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -638,11 +638,11 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_POSCTRL) + if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL) led_color_4 = LED_GREEN; else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTRL) + else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e5124a31f..8c7f6270d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -435,13 +435,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTRL) { - /* ALTCTRL */ - main_res = main_state_transition(status, MAIN_STATE_ALTCTRL); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { + /* ALTCTL */ + main_res = main_state_transition(status, MAIN_STATE_ALTCTL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTRL) { - /* POSCTRL */ - main_res = main_state_transition(status, MAIN_STATE_POSCTRL); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { + /* POSCTL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -456,8 +456,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* POSCTRL */ - main_res = main_state_transition(status, MAIN_STATE_POSCTRL); + /* POSCTL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -634,8 +634,8 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "ALTCTRL"; - main_states_str[2] = "POSCTRL"; + main_states_str[1] = "ALTCTL"; + main_states_str[2] = "POSCTL"; main_states_str[3] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -1335,8 +1335,8 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assist switch is on posctrl position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctrl_switch == SWITCH_POS_ON) { + /* flight termination in manual mode if assist switch is on POSCTL position */ + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1593,25 +1593,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSIST - if (sp_man->posctrl_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_POSCTRL); + if (sp_man->posctl_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to ALTCTRL - print_reject_mode(status, "POSCTRL"); + // else fallback to ALTCTL + print_reject_mode(status, "POSCTL"); } - res = main_state_transition(status, MAIN_STATE_ALTCTRL); + res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->posctrl_switch != SWITCH_POS_ON) { - print_reject_mode(status, "ALTCTRL"); + if (sp_man->posctl_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTCTL"); } // else fallback to MANUAL @@ -1626,9 +1626,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to ALTCTRL (POSCTRL likely will not work too) + // else fallback to ALTCTL (POSCTL likely will not work too) print_reject_mode(status, "AUTO"); - res = main_state_transition(status, MAIN_STATE_ALTCTRL); + res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1674,7 +1674,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_ALTCTRL: + case MAIN_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -1685,7 +1685,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_POSCTRL: + case MAIN_STATE_POSCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 18586053b..ee0d08391 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -320,15 +320,15 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) // MANUAL to ALTCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_ALTCTRL; + new_main_state = MAIN_STATE_ALTCTL; ut_assert("tranisition: manual to altctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: altctrl", MAIN_STATE_ALTCTRL == current_state.main_state); + ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state); // MANUAL to ALTCTRL, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_ALTCTRL; + new_main_state = MAIN_STATE_ALTCTL; ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); @@ -336,15 +336,15 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) // MANUAL to POSCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_POSCTRL; + new_main_state = MAIN_STATE_POSCTL; ut_assert("transition: manual to posctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: posctrl", MAIN_STATE_POSCTRL == current_state.main_state); + ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state); // MANUAL to POSCTRL, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_POSCTRL; + new_main_state = MAIN_STATE_POSCTL; ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index e6274fb89..a83c81850 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, - PX4_CUSTOM_MAIN_MODE_ALTCTRL, - PX4_CUSTOM_MAIN_MODE_POSCTRL, + PX4_CUSTOM_MAIN_MODE_ALTCTL, + PX4_CUSTOM_MAIN_MODE_POSCTL, PX4_CUSTOM_MAIN_MODE_AUTO, }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3b6d95135..97a214c33 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -215,7 +215,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; - case MAIN_STATE_ALTCTRL: + case MAIN_STATE_ALTCTL: /* need at minimum altitude estimate */ if (!status->is_rotary_wing || @@ -226,7 +226,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_POSCTRL: + case MAIN_STATE_POSCTL: /* need at minimum local position estimate */ if (status->condition_local_position_valid || diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index 596e286a4..bbb39f20f 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.main_state == MAIN_STATE_ALTCTRL || - _status.main_state == MAIN_STATE_POSCTRL /* TODO, implement pos control */) { + } else if (_status.main_state == MAIN_STATE_ALTCTL || + _status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 9a7e636c3..b8879157e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_ALTCTRL) { + } else if (status->main_state == MAIN_STATE_ALTCTL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTRL; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; - } else if (status->main_state == MAIN_STATE_POSCTRL) { + } else if (status->main_state == MAIN_STATE_POSCTL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTRL; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 6d46db9bd..91230a37c 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_ALTCTRL || - _status.main_state == MAIN_STATE_POSCTRL) { + _status.main_state == MAIN_STATE_ALTCTL || + _status.main_state == MAIN_STATE_POSCTL) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 727be9492..ac394786d 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -77,8 +77,8 @@ struct manual_control_setpoint_s { float aux5; /**< default function: payload drop */ switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ - switch_pos_t return_switch; /**< rturn to launch 2 position switch (mandatory): no effect, return */ - switch_pos_t posctrl_switch; /**< posctrl 2 position switch (optional): altctrl, posctrl */ + switch_pos_t return_switch; /**< rturn to launch 2 position switch (mandatory): normal, return */ + switch_pos_t posctl_switch; /**< posctrl 2 position switch (optional): altctl, posctl */ switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index f401140ae..f56355246 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -63,8 +63,8 @@ /* main state machine */ typedef enum { MAIN_STATE_MANUAL = 0, - MAIN_STATE_ALTCTRL, - MAIN_STATE_POSCTRL, + MAIN_STATE_ALTCTL, + MAIN_STATE_POSCTL, MAIN_STATE_AUTO, MAIN_STATE_MAX } main_state_t; -- cgit v1.2.3 From 6d9ea86bc9d50d84fcfd8625676e8ff1c53ca1fd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 12 May 2014 09:23:51 +0200 Subject: mavlink receiver: use new manual control setpoint variable names and fix sending of manual control setpoint mavlink message --- src/modules/mavlink/mavlink_messages.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src/modules/mavlink/mavlink_messages.cpp') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 9c552515d..c2490c781 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1138,10 +1138,10 @@ protected: if (manual_sub->update(t)) { mavlink_msg_manual_control_send(_channel, mavlink_system.sysid, - manual->roll * 1000, - manual->pitch * 1000, - manual->yaw * 1000, - manual->throttle * 1000, + manual->x * 1000, + manual->y * 1000, + manual->z * 1000, + manual->r * 1000, 0); } } -- cgit v1.2.3