fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 11:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Moved math library to library dir, improved sensor-level HIL, cleaned up geo / conversion libs

Browse Source
This commit is contained in:
Lorenz Meier
2013-08-21 18:13:01 +02:00
parent 309ea81460
commit fab110d21f
69 changed files with 216 additions and 171 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink.c
+3
View File
@@ -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:
src/modules/mavlink/mavlink_receiver.cpp
+86 -2
View File
@@ -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) {
src/modules/mavlink/orb_listener.c
+41 -9
View File
@@ -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 --- */
src/modules/mavlink/orb_topics.h
+1
View File
@@ -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;