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

Merged beta into paul_estimator

Browse Source
This commit is contained in:
Lorenz Meier
2014-02-16 17:07:02 +01:00
parent 30612eb32d 5bb004a711
commit 8016032a40
206 changed files with 12072 additions and 9311 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/sensors.cpp
+35 -33
View File
@@ -211,8 +211,8 @@ private:
struct differential_pressure_s _diff_pres;
struct airspeed_s _airspeed;
math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
bool _mag_is_external; /**< true if the active mag is on an external board */
uint64_t _battery_discharged; /**< battery discharged current in mA*ms */
@@ -470,8 +470,6 @@ Sensors::Sensors() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
_board_rotation(3, 3),
_external_mag_rotation(3, 3),
_mag_is_external(false),
_battery_discharged(0),
_battery_current_timestamp(0)
@@ -639,41 +637,43 @@ Sensors::parameters_update()
if (!rc_valid)
warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
const char *paramerr = "FAIL PARM LOAD";
/* channel mapping */
if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
warnx("Failed getting roll chan index");
warnx(paramerr);
}
if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) {
warnx("Failed getting pitch chan index");
warnx(paramerr);
}
if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) {
warnx("Failed getting yaw chan index");
warnx(paramerr);
}
if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) {
warnx("Failed getting throttle chan index");
warnx(paramerr);
}
if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
warnx("Failed getting mode sw chan index");
warnx(paramerr);
}
if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
warnx("Failed getting return sw chan index");
warnx(paramerr);
}
if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) {
warnx("Failed getting assisted sw chan index");
warnx(paramerr);
}
if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) {
warnx("Failed getting mission sw chan index");
warnx(paramerr);
}
if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
warnx("Failed getting flaps chan index");
warnx(paramerr);
}
// if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
@@ -745,12 +745,12 @@ Sensors::parameters_update()
/* scaling of ADC ticks to battery voltage */
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
warnx("Failed updating voltage scaling param");
warnx(paramerr);
}
/* scaling of ADC ticks to battery current */
if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
warnx("Failed updating current scaling param");
warnx(paramerr);
}
param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
@@ -931,7 +931,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
vect = _board_rotation * vect;
raw.accelerometer_m_s2[0] = vect(0);
@@ -957,7 +957,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
vect = _board_rotation * vect;
raw.gyro_rad_s[0] = vect(0);
@@ -983,7 +983,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
if (_mag_is_external)
vect = _external_mag_rotation * vect;
@@ -1416,24 +1416,26 @@ Sensors::rc_poll()
}
}
/* mode switch input */
if (_rc.function[MODE] >= 0) {
manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
}
if (_rc.function[MISSION] >= 0) {
manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
}
/* land switch input */
if (_rc.function[RETURN] >= 0) {
manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
}
/* assisted switch input */
if (_rc.function[ASSISTED] >= 0) {
manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
}
/* mission switch input */
if (_rc.function[MISSION] >= 0) {
manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
}
/* return switch input */
if (_rc.function[RETURN] >= 0) {
manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
}
// if (_rc.function[OFFBOARD_MODE] >= 0) {
// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
// }
@@ -1648,17 +1650,17 @@ int sensors_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (sensors::g_sensors != nullptr)
errx(0, "sensors task already running");
errx(0, "already running");
sensors::g_sensors = new Sensors;
if (sensors::g_sensors == nullptr)
errx(1, "sensors task alloc failed");
errx(1, "alloc failed");
if (OK != sensors::g_sensors->start()) {
delete sensors::g_sensors;
sensors::g_sensors = nullptr;
err(1, "sensors task start failed");
err(1, "start failed");
}
exit(0);
@@ -1666,7 +1668,7 @@ int sensors_main(int argc, char *argv[])
if (!strcmp(argv[1], "stop")) {
if (sensors::g_sensors == nullptr)
errx(1, "sensors task not running");
errx(1, "not running");
delete sensors::g_sensors;
sensors::g_sensors = nullptr;
@@ -1675,10 +1677,10 @@ int sensors_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (sensors::g_sensors) {
errx(0, "task is running");
errx(0, "is running");
} else {
errx(1, "task is not running");
errx(1, "not running");
}
}