diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-21 18:13:01 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-21 18:13:01 +0200 |
commit | fab110d21f147e5064ff140aadac649017fa466e (patch) | |
tree | b41291188db4e5734353f0d09a0af306c58ed9f5 /src/modules/mavlink | |
parent | 309ea8146055528c22395fca06b7a70b660c22b3 (diff) | |
download | px4-firmware-fab110d21f147e5064ff140aadac649017fa466e.tar.gz px4-firmware-fab110d21f147e5064ff140aadac649017fa466e.tar.bz2 px4-firmware-fab110d21f147e5064ff140aadac649017fa466e.zip |
Moved math library to library dir, improved sensor-level HIL, cleaned up geo / conversion libs
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r-- | src/modules/mavlink/mavlink.c | 3 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 88 | ||||
-rw-r--r-- | src/modules/mavlink/orb_listener.c | 50 | ||||
-rw-r--r-- | src/modules/mavlink/orb_topics.h | 1 |
4 files changed, 131 insertions, 11 deletions
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 3d3434670..f39bbaa72 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -279,6 +279,9 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m orb_set_interval(subs->act_2_sub, min_interval); orb_set_interval(subs->act_3_sub, min_interval); orb_set_interval(subs->actuators_sub, min_interval); + orb_set_interval(subs->actuators_effective_sub, min_interval); + orb_set_interval(subs->spa_sub, min_interval); + orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval); break; case MAVLINK_MSG_ID_MANUAL_CONTROL: diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 01bbabd46..28f7af33c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -50,6 +50,10 @@ #include <mqueue.h> #include <string.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_accel.h> +#include <drivers/drv_gyro.h> +#include <drivers/drv_mag.h> +#include <drivers/drv_baro.h> #include <time.h> #include <float.h> #include <unistd.h> @@ -101,6 +105,10 @@ static orb_advert_t pub_hil_global_pos = -1; static orb_advert_t pub_hil_attitude = -1; static orb_advert_t pub_hil_gps = -1; static orb_advert_t pub_hil_sensors = -1; +static orb_advert_t pub_hil_gyro = -1; +static orb_advert_t pub_hil_accel = -1; +static orb_advert_t pub_hil_mag = -1; +static orb_advert_t pub_hil_baro = -1; static orb_advert_t pub_hil_airspeed = -1; static orb_advert_t cmd_pub = -1; @@ -412,12 +420,12 @@ handle_message(mavlink_message_t *msg) /* airspeed from differential pressure, ambient pressure and temp */ struct airspeed_s airspeed; - airspeed.timestamp = hrt_absolute_time(); float ias = calc_indicated_airspeed(imu.diff_pressure); // XXX need to fix this float tas = ias; + airspeed.timestamp = hrt_absolute_time(); airspeed.indicated_airspeed_m_s = ias; airspeed.true_airspeed_m_s = tas; @@ -428,7 +436,67 @@ handle_message(mavlink_message_t *msg) } //warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); - /* publish */ + /* individual sensor publications */ + struct gyro_report gyro; + gyro.x_raw = imu.xgyro / mrad2rad; + gyro.y_raw = imu.ygyro / mrad2rad; + gyro.z_raw = imu.zgyro / mrad2rad; + gyro.x = imu.xgyro; + gyro.y = imu.ygyro; + gyro.z = imu.zgyro; + gyro.temperature = imu.temperature; + gyro.timestamp = hrt_absolute_time(); + + if (pub_hil_gyro < 0) { + pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro); + } else { + orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro); + } + + struct accel_report accel; + accel.x_raw = imu.xacc / mg2ms2; + accel.y_raw = imu.yacc / mg2ms2; + accel.z_raw = imu.zacc / mg2ms2; + accel.x = imu.xacc; + accel.y = imu.yacc; + accel.z = imu.zacc; + accel.temperature = imu.temperature; + accel.timestamp = hrt_absolute_time(); + + if (pub_hil_accel < 0) { + pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + } else { + orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); + } + + struct mag_report mag; + mag.x_raw = imu.xmag / mga2ga; + mag.y_raw = imu.ymag / mga2ga; + mag.z_raw = imu.zmag / mga2ga; + mag.x = imu.xmag; + mag.y = imu.ymag; + mag.z = imu.zmag; + mag.timestamp = hrt_absolute_time(); + + if (pub_hil_mag < 0) { + pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag); + } else { + orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag); + } + + struct baro_report baro; + baro.pressure = imu.abs_pressure; + baro.altitude = imu.pressure_alt; + baro.temperature = imu.temperature; + baro.timestamp = hrt_absolute_time(); + + if (pub_hil_baro < 0) { + pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro); + } else { + orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro); + } + + /* publish combined sensor topic */ if (pub_hil_sensors > 0) { orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); } else { @@ -552,6 +620,22 @@ handle_message(mavlink_message_t *msg) } else { pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); } + + struct accel_report accel; + accel.x_raw = hil_state.xacc / 9.81f * 1e3f; + accel.y_raw = hil_state.yacc / 9.81f * 1e3f; + accel.z_raw = hil_state.zacc / 9.81f * 1e3f; + accel.x = hil_state.xacc; + accel.y = hil_state.yacc; + accel.z = hil_state.zacc; + accel.temperature = 25.0f; + accel.timestamp = hrt_absolute_time(); + + if (pub_hil_accel < 0) { + pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + } else { + orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); + } } if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 2a260861d..c711b1803 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -71,7 +71,8 @@ struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_armed_s armed; -struct actuator_controls_effective_s actuators_0; +struct actuator_controls_effective_s actuators_effective_0; +struct actuator_controls_s actuators_0; struct vehicle_attitude_s att; struct mavlink_subscriptions mavlink_subs; @@ -112,6 +113,7 @@ static void l_attitude_setpoint(const struct listener *l); static void l_actuator_outputs(const struct listener *l); static void l_actuator_armed(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l); +static void l_vehicle_attitude_controls_effective(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l); static void l_debug_key_value(const struct listener *l); static void l_optical_flow(const struct listener *l); @@ -138,6 +140,7 @@ static const struct listener listeners[] = { {l_actuator_armed, &mavlink_subs.armed_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, + {l_vehicle_attitude_controls_effective, &mavlink_subs.actuators_effective_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, {l_optical_flow, &mavlink_subs.optical_flow, 0}, {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, @@ -567,28 +570,54 @@ l_manual_control_setpoint(const struct listener *l) } void -l_vehicle_attitude_controls(const struct listener *l) +l_vehicle_attitude_controls_effective(const struct listener *l) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0); if (gcs_link) { /* send, add spaces so that string buffer is at least 10 chars long */ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl0 ", - actuators_0.control_effective[0]); + actuators_effective_0.control_effective[0]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl1 ", - actuators_0.control_effective[1]); + actuators_effective_0.control_effective[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl2 ", - actuators_0.control_effective[2]); + actuators_effective_0.control_effective[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl3 ", - actuators_0.control_effective[3]); + actuators_effective_0.control_effective[3]); + } +} + +void +l_vehicle_attitude_controls(const struct listener *l) +{ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0); + + if (gcs_link) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl0 ", + actuators_0.control[0]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl1 ", + actuators_0.control[1]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl2 ", + actuators_0.control[2]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl3 ", + actuators_0.control[3]); } } @@ -637,7 +666,7 @@ l_airspeed(const struct listener *l) orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1); + float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); float alt = global_pos.alt; float climb = global_pos.vz; @@ -773,7 +802,10 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ /* --- ACTUATOR CONTROL VALUE --- */ - mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + orb_set_interval(mavlink_subs.actuators_effective_sub, 100); /* 10Hz updates */ + + mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ /* --- DEBUG VALUE OUTPUT --- */ diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 506f73105..e2e630046 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -80,6 +80,7 @@ struct mavlink_subscriptions { int safety_sub; int actuators_sub; int armed_sub; + int actuators_effective_sub; int local_pos_sub; int spa_sub; int spl_sub; |