mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-22 01:12:31 +00:00
Faster sensor bus resets on timeouts, massively reworked fixed wing app, tested
This commit is contained in:
Lorenz Meier
@@ -8,7 +8,7 @@ echo "[init] doing standalone PX4FMU startup..."
|
||||
#
|
||||
# Start the ORB
|
||||
#
|
||||
#uorb start
|
||||
uorb start
|
||||
|
||||
#
|
||||
# Start the sensors.
|
||||
|
||||
@@ -32,7 +32,8 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Driver for the MS5611 barometric pressure sensor connected via I2C
|
||||
* @file ms5611.cpp
|
||||
* Driver for the MS5611 barometric pressure sensor connected via I2C.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
@@ -37,7 +37,7 @@
|
||||
|
||||
APPNAME = fixedwing_control
|
||||
PRIORITY = SCHED_PRIORITY_MAX - 1
|
||||
STACKSIZE = 12288
|
||||
STACKSIZE = 4096
|
||||
|
||||
CSRCS = fixedwing_control.c
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Ivan Ovinnikov <oivan@ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
@@ -53,7 +53,17 @@
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <nuttx/spi.h>
|
||||
#include "../mix_and_link/mix_and_link.h"
|
||||
#include "fixedwing_control.h"
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_global_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/fixedwing_control.h>
|
||||
|
||||
#ifndef F_M_PI
|
||||
#define F_M_PI ((float)M_PI)
|
||||
#endif
|
||||
|
||||
__EXPORT int fixedwing_control_main(int argc, char *argv[]);
|
||||
|
||||
@@ -61,7 +71,6 @@ __EXPORT int fixedwing_control_main(int argc, char *argv[]);
|
||||
#define PID_SCALER 0.1f
|
||||
#define PID_DERIVMODE_CALC 0
|
||||
#define HIL_MODE 32
|
||||
#define RAD2DEG ((1.0/180.0)*M_PI)
|
||||
#define AUTO -1000
|
||||
#define MANUAL 3000
|
||||
#define SERVO_MIN 1000
|
||||
@@ -75,7 +84,6 @@ pthread_t servo_thread;
|
||||
* Servo channels function enumerator used for
|
||||
* the servo writing part
|
||||
*/
|
||||
|
||||
enum SERVO_CHANNELS_FUNCTION {
|
||||
|
||||
AIL_1 = 0,
|
||||
@@ -127,9 +135,9 @@ typedef struct {
|
||||
|
||||
/* Next waypoint*/
|
||||
|
||||
float wp_x;
|
||||
float wp_y;
|
||||
float wp_z;
|
||||
double wp_x;
|
||||
double wp_y;
|
||||
double wp_z;
|
||||
|
||||
/* Setpoints */
|
||||
|
||||
@@ -189,14 +197,6 @@ static float calc_throttle_setpoint(void);
|
||||
static float calc_wp_distance(void);
|
||||
static void set_plane_mode(void);
|
||||
|
||||
/*
|
||||
* The control, navigation and servo loop threads
|
||||
*/
|
||||
|
||||
static void *control_loop(void *arg);
|
||||
static void *nav_loop(void *arg);
|
||||
static void *servo_loop(void *arg);
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
@@ -210,8 +210,9 @@ float scaler = 1; //M_PI;
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
*
|
||||
* Calculates the PID control output given an error. Incorporates PD scaling and low-pass filter for the derivative component.
|
||||
* Calculates the PID control output given an error.
|
||||
*
|
||||
* Incorporates PD scaling and low-pass filter for the derivative component.
|
||||
*
|
||||
* @param error the input error
|
||||
* @param error_deriv the derivative of the input error
|
||||
@@ -241,7 +242,7 @@ static float pid(float error, float error_deriv, uint16_t dt, float scale, float
|
||||
|
||||
output += error * Kp;
|
||||
|
||||
if ((fabs(Kd) > 0) && (dt > 0)) {
|
||||
if ((fabsf(Kd) > 0) && (dt > 0)) {
|
||||
|
||||
if (PID_DERIVMODE_CALC) {
|
||||
derivative = (error - lerror) / delta_time;
|
||||
@@ -250,7 +251,7 @@ static float pid(float error, float error_deriv, uint16_t dt, float scale, float
|
||||
* discrete low pass filter, cuts out the
|
||||
* high frequency noise that can drive the controller crazy
|
||||
*/
|
||||
float RC = 1 / (2 * M_PI * fCut);
|
||||
float RC = 1.0 / (2.0f * M_PI * fCut);
|
||||
derivative = lderiv +
|
||||
(delta_time / (RC + delta_time)) * (derivative - lderiv);
|
||||
|
||||
@@ -272,7 +273,7 @@ static float pid(float error, float error_deriv, uint16_t dt, float scale, float
|
||||
output *= scale;
|
||||
|
||||
/* Compute integral component if time has elapsed */
|
||||
if ((fabs(Ki) > 0) && (dt > 0)) {
|
||||
if ((fabsf(Ki) > 0) && (dt > 0)) {
|
||||
integrator += (error * Ki) * scaler * delta_time;
|
||||
|
||||
if (integrator < -imax) {
|
||||
@@ -331,8 +332,8 @@ static void get_parameters()
|
||||
static void calc_body_angular_rates(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed)
|
||||
{
|
||||
plane_data.p = rollspeed - sinf(pitch) * yawspeed;
|
||||
plane_data.q = cosf(roll) * pitchspeed + sinf(roll) * cos(pitch) * yawspeed;
|
||||
plane_data.r = -sinf(roll) * pitchspeed + cosf(roll) * cos(pitch) * yawspeed;
|
||||
plane_data.q = cosf(roll) * pitchspeed + sinf(roll) * cosf(pitch) * yawspeed;
|
||||
plane_data.r = -sinf(roll) * pitchspeed + cosf(roll) * cosf(pitch) * yawspeed;
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -390,13 +391,13 @@ static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, fl
|
||||
* @return bearing Bearing error
|
||||
*
|
||||
*/
|
||||
|
||||
static float calc_bearing()
|
||||
{
|
||||
float bearing = 90 + atan2(-(plane_data.wp_y - plane_data.lat), (plane_data.wp_x - plane_data.lon)) * RAD2DEG;
|
||||
float bearing = F_M_PI/2.0f + (float)atan2(-(plane_data.wp_y - plane_data.lat), (plane_data.wp_x - plane_data.lon));
|
||||
|
||||
if (bearing < 0)
|
||||
bearing += 360;
|
||||
if (bearing < 0.0f) {
|
||||
bearing += 2*F_M_PI;
|
||||
}
|
||||
|
||||
return bearing;
|
||||
}
|
||||
@@ -430,7 +431,6 @@ static float calc_roll_ail()
|
||||
*
|
||||
* @return Pitch elevators control output (-1,1)
|
||||
*/
|
||||
|
||||
static float calc_pitch_elev()
|
||||
{
|
||||
float ret = pid((plane_data.pitch_setpoint - plane_data.pitch) / scaler, plane_data.pitchspeed, PID_DT, PID_SCALER,
|
||||
@@ -453,10 +453,9 @@ static float calc_pitch_elev()
|
||||
* @return Yaw rudder control output (-1,1)
|
||||
*
|
||||
*/
|
||||
|
||||
static float calc_yaw_rudder(float hdg)
|
||||
{
|
||||
float ret = pid((plane_data.yaw / RAD2DEG - abs(hdg)) / scaler, plane_data.yawspeed, PID_DT, PID_SCALER,
|
||||
float ret = pid((plane_data.yaw - abs(hdg)) / scaler, plane_data.yawspeed, PID_DT, PID_SCALER,
|
||||
plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos);
|
||||
|
||||
if (ret < -1)
|
||||
@@ -481,11 +480,11 @@ static float calc_throttle()
|
||||
float ret = pid(plane_data.throttle_setpoint - calc_gnd_speed(), 0, PID_DT, PID_SCALER,
|
||||
plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos);
|
||||
|
||||
if (ret < 0.2)
|
||||
return 0.2;
|
||||
if (ret < 0.2f)
|
||||
return 0.2f;
|
||||
|
||||
if (ret > 1)
|
||||
return 1;
|
||||
if (ret > 1.0f)
|
||||
return 1.0f;
|
||||
|
||||
return ret;
|
||||
}
|
||||
@@ -542,11 +541,11 @@ static float calc_roll_setpoint()
|
||||
} else {
|
||||
setpoint = calc_bearing() - plane_data.yaw;
|
||||
|
||||
if (setpoint < -35)
|
||||
return -35;
|
||||
if (setpoint < (-35.0f/180.0f)*F_M_PI)
|
||||
return (-35.0f/180.0f)*F_M_PI;
|
||||
|
||||
if (setpoint > 35)
|
||||
return 35;
|
||||
if (setpoint > (35/180.0f)*F_M_PI)
|
||||
return (-35.0f/180.0f)*F_M_PI;
|
||||
}
|
||||
|
||||
return setpoint;
|
||||
@@ -564,19 +563,19 @@ static float calc_roll_setpoint()
|
||||
|
||||
static float calc_pitch_setpoint()
|
||||
{
|
||||
float setpoint = 0;
|
||||
float setpoint = 0.0f;
|
||||
|
||||
if (plane_data.mode == TAKEOFF) {
|
||||
setpoint = 35;
|
||||
setpoint = ((35/180.0f)*F_M_PI);
|
||||
|
||||
} else {
|
||||
setpoint = atanf((plane_data.wp_z - plane_data.alt) / calc_wp_distance()) * RAD2DEG;
|
||||
setpoint = atanf((plane_data.wp_z - plane_data.alt) / calc_wp_distance());
|
||||
|
||||
if (setpoint < -35)
|
||||
return -35;
|
||||
if (setpoint < (-35.0f/180.0f)*F_M_PI)
|
||||
return (-35.0f/180.0f)*F_M_PI;
|
||||
|
||||
if (setpoint > 35)
|
||||
return 35;
|
||||
if (setpoint > (35/180.0f)*F_M_PI)
|
||||
return (-35.0f/180.0f)*F_M_PI;
|
||||
}
|
||||
|
||||
return setpoint;
|
||||
@@ -597,7 +596,7 @@ static float calc_throttle_setpoint()
|
||||
|
||||
// if TAKEOFF full throttle
|
||||
if (plane_data.mode == TAKEOFF) {
|
||||
setpoint = 60;
|
||||
setpoint = 0.6f;
|
||||
}
|
||||
|
||||
// if CRUISE - parameter value
|
||||
@@ -607,7 +606,7 @@ static float calc_throttle_setpoint()
|
||||
|
||||
// if LAND no throttle
|
||||
if (plane_data.mode == LAND) {
|
||||
setpoint = 0;
|
||||
setpoint = 0.0f;
|
||||
}
|
||||
|
||||
return setpoint;
|
||||
@@ -623,7 +622,7 @@ static float calc_throttle_setpoint()
|
||||
|
||||
static void set_plane_mode()
|
||||
{
|
||||
if (plane_data.alt < 10) {
|
||||
if (plane_data.alt < 10.0f) {
|
||||
plane_data.mode = TAKEOFF;
|
||||
|
||||
} else {
|
||||
@@ -647,27 +646,31 @@ static void set_plane_mode()
|
||||
|
||||
int fixedwing_control_main(int argc, char *argv[])
|
||||
{
|
||||
/* print text */
|
||||
printf("Fixedwing control started\n");
|
||||
usleep(100000);
|
||||
|
||||
/* default values for arguments */
|
||||
char *fixedwing_uart_name = "/dev/ttyS1";
|
||||
char *commandline_usage = "\tusage: fixedwing_control -d fixedwing-devicename\n";
|
||||
char *commandline_usage = "\tusage: %s -d fixedwing-devicename\n";
|
||||
|
||||
/* read arguments */
|
||||
int i;
|
||||
bool verbose = false;
|
||||
|
||||
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
|
||||
if (argc > i + 1) {
|
||||
fixedwing_uart_name = argv[i + 1];
|
||||
for (int i = 1; i < argc; i++) {
|
||||
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
|
||||
if (argc > i + 1) {
|
||||
fixedwing_uart_name = argv[i + 1];
|
||||
|
||||
} else {
|
||||
printf(commandline_usage);
|
||||
return 0;
|
||||
} else {
|
||||
printf(commandline_usage, argv[0]);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
|
||||
verbose = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* welcome user */
|
||||
printf("[fixedwing control] started\n");
|
||||
|
||||
/* Set up to publish fixed wing control messages */
|
||||
struct fixedwing_control_s control;
|
||||
int fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control);
|
||||
@@ -679,6 +682,12 @@ int fixedwing_control_main(int argc, char *argv[])
|
||||
int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
struct rc_channels_s rc;
|
||||
int rc_sub = orb_subscribe(ORB_ID(rc_channels));
|
||||
struct vehicle_global_position_setpoint_s global_setpoint;
|
||||
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
|
||||
|
||||
/* Mainloop setup */
|
||||
unsigned int loopcounter = 0;
|
||||
unsigned int failcounter = 0;
|
||||
|
||||
/* Control constants */
|
||||
control_outputs.mode = HIL_MODE;
|
||||
@@ -692,7 +701,7 @@ int fixedwing_control_main(int argc, char *argv[])
|
||||
fd = open("/dev/pwm_servo", O_RDWR);
|
||||
|
||||
if (fd < 0) {
|
||||
printf("failed opening /dev/pwm_servo\n");
|
||||
printf("[fixedwing control] Failed opening /dev/pwm_servo\n");
|
||||
}
|
||||
|
||||
ioctl(fd, PWM_SERVO_ARM, 0);
|
||||
@@ -728,8 +737,7 @@ int fixedwing_control_main(int argc, char *argv[])
|
||||
* Main control, navigation and servo routine
|
||||
*/
|
||||
|
||||
while(1)
|
||||
{
|
||||
while(1) {
|
||||
/*
|
||||
* DATA Handling
|
||||
* Fetch current flight data
|
||||
@@ -738,6 +746,7 @@ int fixedwing_control_main(int argc, char *argv[])
|
||||
/* get position, attitude and rc inputs */
|
||||
// XXX add error checking
|
||||
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
|
||||
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
|
||||
orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &att);
|
||||
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
|
||||
|
||||
@@ -746,12 +755,12 @@ int fixedwing_control_main(int argc, char *argv[])
|
||||
*/
|
||||
|
||||
/* position values*/
|
||||
plane_data.lat = global_pos.lat / 10000000;
|
||||
plane_data.lon = global_pos.lon / 10000000;
|
||||
plane_data.alt = global_pos.alt / 1000;
|
||||
plane_data.vx = global_pos.vx / 100;
|
||||
plane_data.vy = global_pos.vy / 100;
|
||||
plane_data.vz = global_pos.vz / 100;
|
||||
plane_data.lat = global_pos.lat / 10000000.0;
|
||||
plane_data.lon = global_pos.lon / 10000000.0;
|
||||
plane_data.alt = global_pos.alt / 1000.0f;
|
||||
plane_data.vx = global_pos.vx / 100.0f;
|
||||
plane_data.vy = global_pos.vy / 100.0f;
|
||||
plane_data.vz = global_pos.vz / 100.0f;
|
||||
|
||||
/* attitude values*/
|
||||
plane_data.roll = att.roll;
|
||||
@@ -766,37 +775,35 @@ int fixedwing_control_main(int argc, char *argv[])
|
||||
|
||||
/* Attitude control part */
|
||||
|
||||
//#define MUTE
|
||||
#ifndef MUTE
|
||||
/******************************** DEBUG OUTPUT ************************************************************/
|
||||
if (verbose && loopcounter % 20 == 0) {
|
||||
/******************************** DEBUG OUTPUT ************************************************************/
|
||||
|
||||
printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)plane_data.Kp_att, (int)plane_data.Ki_att,
|
||||
(int)plane_data.Kd_att, (int)plane_data.intmax_att, (int)plane_data.Kp_pos, (int)plane_data.Ki_pos,
|
||||
(int)plane_data.Kd_pos, (int)plane_data.intmax_pos, (int)plane_data.airspeed,
|
||||
(int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z);
|
||||
printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)plane_data.Kp_att, (int)plane_data.Ki_att,
|
||||
(int)plane_data.Kd_att, (int)plane_data.intmax_att, (int)plane_data.Kp_pos, (int)plane_data.Ki_pos,
|
||||
(int)plane_data.Kd_pos, (int)plane_data.intmax_pos, (int)plane_data.airspeed,
|
||||
(int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z);
|
||||
|
||||
// printf("PITCH SETPOINT: %i\n", (int)plane_data.pitch_setpoint);
|
||||
// printf("ROLL SETPOINT: %i\n", (int)plane_data.roll_setpoint);
|
||||
// printf("THROTTLE SETPOINT: %i\n", (int)calc_throttle_setpoint());
|
||||
// printf("PITCH SETPOINT: %i\n", (int)plane_data.pitch_setpoint);
|
||||
// printf("ROLL SETPOINT: %i\n", (int)plane_data.roll_setpoint);
|
||||
// printf("THROTTLE SETPOINT: %i\n", (int)calc_throttle_setpoint());
|
||||
|
||||
// printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed()));
|
||||
// printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed()));
|
||||
|
||||
// printf("Current Altitude: %i\n\n", (int)plane_data.alt);
|
||||
// printf("Current Altitude: %i\n\n", (int)plane_data.alt);
|
||||
|
||||
printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ",
|
||||
(int)(1000 * plane_data.roll), (int)(1000 * plane_data.pitch), (int)(1000 * plane_data.yaw),
|
||||
(int)(100 * plane_data.rollspeed), (int)(100 * plane_data.pitchspeed), (int)(100 * plane_data.yawspeed));
|
||||
printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ",
|
||||
(int)(1000 * plane_data.roll), (int)(1000 * plane_data.pitch), (int)(1000 * plane_data.yaw),
|
||||
(int)(100 * plane_data.rollspeed), (int)(100 * plane_data.pitchspeed), (int)(100 * plane_data.yawspeed));
|
||||
|
||||
// printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n",
|
||||
// (int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r));
|
||||
// printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n",
|
||||
// (int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r));
|
||||
|
||||
printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n",
|
||||
(int)(10000 * control_outputs.roll_ailerons), (int)(10000 * control_outputs.pitch_elevator),
|
||||
(int)(10000 * control_outputs.yaw_rudder), (int)(10000 * control_outputs.throttle));
|
||||
printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n",
|
||||
(int)(10000 * control_outputs.roll_ailerons), (int)(10000 * control_outputs.pitch_elevator),
|
||||
(int)(10000 * control_outputs.yaw_rudder), (int)(10000 * control_outputs.throttle));
|
||||
|
||||
/************************************************************************************************************/
|
||||
|
||||
#endif
|
||||
/************************************************************************************************************/
|
||||
}
|
||||
|
||||
/*
|
||||
* Computation section
|
||||
@@ -809,11 +816,11 @@ int fixedwing_control_main(int argc, char *argv[])
|
||||
set_plane_mode();
|
||||
|
||||
/* Calculate the P,Q,R body rates of the aircraft */
|
||||
calc_body_angular_rates(plane_data.roll / RAD2DEG, plane_data.pitch / RAD2DEG, plane_data.yaw / RAD2DEG,
|
||||
calc_body_angular_rates(plane_data.roll, plane_data.pitch, plane_data.yaw,
|
||||
plane_data.rollspeed, plane_data.pitchspeed, plane_data.yawspeed);
|
||||
|
||||
/* Calculate the body frame angles of the aircraft */
|
||||
//calc_bodyframe_angles(plane_data.roll/RAD2DEG,plane_data.pitch/RAD2DEG,plane_data.yaw/RAD2DEG);
|
||||
//calc_bodyframe_angles(plane_data.roll,plane_data.pitch,plane_data.yaw);
|
||||
|
||||
/* Calculate the output values */
|
||||
control_outputs.roll_ailerons = calc_roll_ail();
|
||||
@@ -869,9 +876,6 @@ int fixedwing_control_main(int argc, char *argv[])
|
||||
|
||||
//control_outputs.yaw_rudder = calc_yaw_rudder(plane_data.hdg);
|
||||
|
||||
// 10 Hz loop
|
||||
usleep(100000);
|
||||
|
||||
} else {
|
||||
control.attitude_control_output[ROLL] = rc.chan[rc.function[ROLL]].scale;
|
||||
control.attitude_control_output[PITCH] = rc.chan[rc.function[PITCH]].scale;
|
||||
@@ -927,9 +931,14 @@ int fixedwing_control_main(int argc, char *argv[])
|
||||
int result = write(fd, &data, sizeof(data));
|
||||
|
||||
if (result != sizeof(data)) {
|
||||
printf("failed writing servo outputs\n");
|
||||
if (failcounter < 10 || failcounter % 20 == 0) {
|
||||
printf("[fixedwing_control] failed writing servo outputs\n");
|
||||
}
|
||||
failcounter++;
|
||||
}
|
||||
|
||||
loopcounter++;
|
||||
|
||||
/* 20Hz loop*/
|
||||
usleep(50000);
|
||||
}
|
||||
|
||||
@@ -1,66 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Ivan Ovinnikov <oivan@ethz.ch>
|
||||
*
|
||||
* 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 Definition of attitude controller
|
||||
*/
|
||||
|
||||
#ifndef FIXEDWING_CONTROL_H_
|
||||
#define FIXEDWING_CONTROL_H_
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/fixedwing_control.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Internal definitions
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
****************************************************************************/
|
||||
|
||||
#endif /* FIXEDWING_CONTROL_H_ */
|
||||
@@ -1,8 +1,7 @@
|
||||
/****************************************************************************
|
||||
* pid.c
|
||||
*
|
||||
* Copyright (C) 2012 Ivan Ovinnikov. All rights reserved.
|
||||
* Authors: Ivan Ovinnikov <oivan@ethz.ch>
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Ivan Ovinnikov <oivan@ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -14,7 +13,7 @@
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name NuttX nor the names of its contributors may be
|
||||
* 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.
|
||||
*
|
||||
@@ -33,9 +32,10 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
/**
|
||||
* @file pid.c
|
||||
* Implementation of a fixed wing attitude and position controller.
|
||||
*/
|
||||
|
||||
#include "pid.h"
|
||||
#include "fixedwing_control.h"
|
||||
|
||||
@@ -213,7 +213,7 @@ extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float pa
|
||||
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp);
|
||||
}
|
||||
|
||||
printf("[mavlink mp] new setpoint: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x, param6_lon_y, param7_alt_z, param4);
|
||||
//printf("[mavlink mp] new setpoint: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000);
|
||||
}
|
||||
|
||||
|
||||
@@ -375,7 +375,7 @@ static void *receiveloop(void *arg)
|
||||
static void *uorb_receiveloop(void *arg)
|
||||
{
|
||||
/* Set thread name */
|
||||
prctl(PR_SET_NAME, "mavlink uORB async", getpid());
|
||||
prctl(PR_SET_NAME, "mavlink uORB", getpid());
|
||||
|
||||
|
||||
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
|
||||
@@ -586,7 +586,10 @@ static void *uorb_receiveloop(void *arg)
|
||||
if (fds[6].revents & POLLIN) {
|
||||
/* copy fixed wing control into local buffer */
|
||||
orb_copy(ORB_ID(fixedwing_control), fw_sub, &fw_control);
|
||||
// XXX Output fixed wing control
|
||||
/* send control output via MAVLink */
|
||||
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, fw_control.timestamp / 1000, fw_control.attitude_control_output[0],
|
||||
fw_control.attitude_control_output[1], fw_control.attitude_control_output[2],
|
||||
fw_control.attitude_control_output[3]);
|
||||
}
|
||||
|
||||
/* --- VEHICLE GLOBAL POSITION --- */
|
||||
@@ -594,15 +597,15 @@ static void *uorb_receiveloop(void *arg)
|
||||
/* copy global position data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
|
||||
uint64_t timestamp = global_pos.timestamp;
|
||||
int32_t lat = (int32_t)(global_pos.lat * 1e7);
|
||||
int32_t lon = (int32_t)(global_pos.lon * 1e7);
|
||||
int32_t alt = (int32_t)(global_pos.alt * 1e3);
|
||||
int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1e3);
|
||||
int16_t vx = (int16_t)(global_pos.vx * 1e2);
|
||||
int16_t vy = (int16_t)(global_pos.vy * 1e2);
|
||||
int16_t vz = (int16_t)(global_pos.vz * 1e2);
|
||||
int32_t lat = global_pos.lat;
|
||||
int32_t lon = global_pos.lon;
|
||||
int32_t alt = (int32_t)(global_pos.alt*1000);
|
||||
int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f);
|
||||
int16_t vx = (int16_t)(global_pos.vx * 100.0f);
|
||||
int16_t vy = (int16_t)(global_pos.vy * 100.0f);
|
||||
int16_t vz = (int16_t)(global_pos.vz * 100.0f);
|
||||
/* heading in degrees * 10, from 0 to 36.000) */
|
||||
uint16_t hdg = (global_pos.hdg / M_PI_F) * (180 * 10) + (180 * 10);
|
||||
uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
|
||||
|
||||
mavlink_msg_global_position_int_send(MAVLINK_COMM_0, timestamp / 1000, lat, lon, alt, relative_alt, vx, vy, vz, hdg);
|
||||
}
|
||||
|
||||
@@ -37,9 +37,6 @@
|
||||
|
||||
APPNAME = attitude_estimator_bm
|
||||
PRIORITY = SCHED_PRIORITY_MAX - 10
|
||||
STACKSIZE = 3000
|
||||
|
||||
# XXX this is *horribly* broken
|
||||
INCLUDES = $(TOPDIR)/../mavlink/include/mavlink
|
||||
STACKSIZE = 4096
|
||||
|
||||
include $(APPDIR)/mk/app.mk
|
||||
|
||||
@@ -33,7 +33,10 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* @file Black Magic Attitude Estimator */
|
||||
/**
|
||||
* @file attitude_estimator_bm.c
|
||||
* Black Magic Attitude Estimator
|
||||
*/
|
||||
|
||||
|
||||
|
||||
@@ -49,6 +52,7 @@
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <math.h>
|
||||
#include <errno.h>
|
||||
|
||||
@@ -135,14 +139,22 @@ int attitude_estimator_bm_main(int argc, char *argv[])
|
||||
/* rate-limit raw data updates to 200Hz */
|
||||
//orb_set_interval(sub_raw, 5);
|
||||
|
||||
bool hil_enabled = false;
|
||||
bool publishing = false;
|
||||
|
||||
/* advertise attitude */
|
||||
int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
|
||||
publishing = true;
|
||||
|
||||
struct pollfd fds[] = {
|
||||
{ .fd = sub_raw, .events = POLLIN },
|
||||
};
|
||||
|
||||
// int paramcounter = 100;
|
||||
/* subscribe to system status */
|
||||
struct vehicle_status_s vstatus = {0};
|
||||
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
|
||||
unsigned int loopcounter = 0;
|
||||
|
||||
/* Main loop*/
|
||||
while (true) {
|
||||
@@ -217,8 +229,29 @@ int attitude_estimator_bm_main(int argc, char *argv[])
|
||||
att.R[0][2] = x_n_b.z;
|
||||
// XXX add remaining entries
|
||||
|
||||
if (loopcounter % 250 == 0) {
|
||||
/* Check HIL state */
|
||||
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
|
||||
/* switching from non-HIL to HIL mode */
|
||||
if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) {
|
||||
hil_enabled = true;
|
||||
publishing = false;
|
||||
close(pub_att);
|
||||
|
||||
/* switching from HIL to non-HIL mode */
|
||||
|
||||
} else if (!publishing && !hil_enabled) {
|
||||
/* advertise the topic and make the initial publication */
|
||||
pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
|
||||
hil_enabled = false;
|
||||
publishing = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Broadcast
|
||||
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
|
||||
if (publishing) orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
|
||||
|
||||
loopcounter++;
|
||||
}
|
||||
|
||||
/* Should never reach here */
|
||||
|
||||
@@ -32,7 +32,8 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Driver for the PX4IO board.
|
||||
* @file px4io.cpp
|
||||
* Driver for the PX4IO board.
|
||||
*
|
||||
* PX4IO is connected via serial (or possibly some other interface at a later
|
||||
* point).
|
||||
@@ -555,6 +556,6 @@ px4io_main(int argc, char *argv[])
|
||||
|
||||
|
||||
|
||||
printf("need a verb, only support 'start'\n");
|
||||
printf("need a verb, only support 'start' and 'update'\n");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
@@ -32,14 +32,10 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Top-level logic for the PX4IO module.
|
||||
* @file px4io.c
|
||||
* Top-level logic for the PX4IO module.
|
||||
*/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
#include <stdbool.h>
|
||||
@@ -58,24 +54,8 @@
|
||||
|
||||
#include "px4io.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
__EXPORT int user_start(int argc, char *argv[]);
|
||||
|
||||
/****************************************************************************
|
||||
* user_start
|
||||
****************************************************************************/
|
||||
|
||||
struct sys_state_s system_state;
|
||||
int gpio_fd;
|
||||
|
||||
|
||||
@@ -90,17 +90,14 @@
|
||||
#define BAT_VOL_LOWPASS_2 0.01f
|
||||
#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
|
||||
|
||||
/*PPM Settings*/
|
||||
/* PPM Settings */
|
||||
#define PPM_MIN 1000
|
||||
#define PPM_MAX 2000
|
||||
/*Our internal resolution is 10000*/
|
||||
/* Internal resolution is 10000 */
|
||||
#define PPM_SCALE 10000/((PPM_MAX-PPM_MIN)/2)
|
||||
|
||||
#define PPM_MID (PPM_MIN+PPM_MAX)/2
|
||||
|
||||
/****************************************************************************
|
||||
* Definitions
|
||||
****************************************************************************/
|
||||
static pthread_cond_t sensors_read_ready;
|
||||
static pthread_mutex_t sensors_read_ready_mutex;
|
||||
|
||||
@@ -281,7 +278,8 @@ int sensors_main(int argc, char *argv[])
|
||||
bool baro_healthy = false;
|
||||
bool adc_healthy = false;
|
||||
|
||||
bool hil_enabled = false;
|
||||
bool hil_enabled = false; /**< HIL is disabled by default */
|
||||
bool publishing = false; /**< the app is not publishing by default, only if HIL is disabled on first run */
|
||||
|
||||
int magcounter = 0;
|
||||
int barocounter = 0;
|
||||
@@ -318,16 +316,19 @@ int sensors_main(int argc, char *argv[])
|
||||
int16_t acc_offset[3] = {0, 0, 0};
|
||||
int16_t gyro_offset[3] = {0, 0, 0};
|
||||
|
||||
#pragma pack(push,1)
|
||||
struct adc_msg4_s {
|
||||
uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */
|
||||
int32_t am_data1; /**< ADC convert result 1 (4 bytes) */
|
||||
uint8_t am_channel2; /**< The 8-bit ADC Channel 2 */
|
||||
int32_t am_data2; /**< ADC convert result 2 (4 bytes) */
|
||||
uint8_t am_channel3; /**< The 8-bit ADC Channel 3 */
|
||||
int32_t am_data3; /**< ADC convert result 3 (4 bytes) */
|
||||
uint8_t am_channel4; /**< The 8-bit ADC Channel 4 */
|
||||
int32_t am_data4; /**< ADC convert result 4 (4 bytes) */
|
||||
} __attribute__((__packed__));;
|
||||
uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */
|
||||
int32_t am_data1; /**< ADC convert result 1 (4 bytes) */
|
||||
uint8_t am_channel2; /**< The 8-bit ADC Channel 2 */
|
||||
int32_t am_data2; /**< ADC convert result 2 (4 bytes) */
|
||||
uint8_t am_channel3; /**< The 8-bit ADC Channel 3 */
|
||||
int32_t am_data3; /**< ADC convert result 3 (4 bytes) */
|
||||
uint8_t am_channel4; /**< The 8-bit ADC Channel 4 */
|
||||
int32_t am_data4; /**< ADC convert result 4 (4 bytes) */
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
struct adc_msg4_s buf_adc;
|
||||
size_t adc_readsize = 1 * sizeof(struct adc_msg4_s);
|
||||
|
||||
@@ -335,7 +336,7 @@ int sensors_main(int argc, char *argv[])
|
||||
battery_voltage_conversion = global_data_parameter_storage->pm.param_values[PARAM_BATTERYVOLTAGE_CONVERSION];
|
||||
|
||||
if (-1.0f == battery_voltage_conversion) {
|
||||
/**< default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
|
||||
/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
|
||||
battery_voltage_conversion = 3.3f * 52.0f / 5.0f / 4095.0f;
|
||||
}
|
||||
|
||||
@@ -383,13 +384,14 @@ int sensors_main(int argc, char *argv[])
|
||||
|
||||
/* advertise the topic and make the initial publication */
|
||||
sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
|
||||
publishing = true;
|
||||
|
||||
/* advertise the rc topic */
|
||||
struct rc_channels_s rc = {0};
|
||||
int rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
|
||||
|
||||
/* subscribe to system status */
|
||||
struct vehicle_status_s vstatus;
|
||||
struct vehicle_status_s vstatus = {0};
|
||||
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
|
||||
|
||||
@@ -430,14 +432,16 @@ int sensors_main(int argc, char *argv[])
|
||||
/* switching from non-HIL to HIL mode */
|
||||
if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) {
|
||||
hil_enabled = true;
|
||||
publishing = false;
|
||||
close(sensor_pub);
|
||||
|
||||
/* switching from HIL to non-HIL mode */
|
||||
|
||||
} else if (!(vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && hil_enabled) {
|
||||
} else if (!publishing && !hil_enabled) {
|
||||
/* advertise the topic and make the initial publication */
|
||||
sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
|
||||
hil_enabled = false;
|
||||
publishing = true;
|
||||
}
|
||||
|
||||
|
||||
@@ -556,7 +560,7 @@ int sensors_main(int argc, char *argv[])
|
||||
if (acctime > 500) printf("ACC: %d us\n", acctime);
|
||||
|
||||
/* MAGNETOMETER */
|
||||
if (magcounter == 4) { //(magcounter == 4) // 100 Hz
|
||||
if (magcounter == 4) { /* 120 Hz */
|
||||
uint64_t start_mag = hrt_absolute_time();
|
||||
ret_magnetometer = read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer));
|
||||
int errcode_mag = (int) * get_errno_ptr();
|
||||
@@ -604,7 +608,7 @@ int sensors_main(int argc, char *argv[])
|
||||
magcounter++;
|
||||
|
||||
/* BAROMETER */
|
||||
if (barocounter == 5 && (fd_barometer > 0)) { //(barocounter == 4) // 100 Hz
|
||||
if (barocounter == 5 && (fd_barometer > 0)) { /* 100 Hz */
|
||||
uint64_t start_baro = hrt_absolute_time();
|
||||
*get_errno_ptr() = 0;
|
||||
ret_barometer = read(fd_barometer, buf_barometer, sizeof(buf_barometer));
|
||||
@@ -798,7 +802,7 @@ int sensors_main(int argc, char *argv[])
|
||||
uint64_t total_time = hrt_absolute_time() - current_time;
|
||||
|
||||
/* Inform other processes that new data is available to copy */
|
||||
if ((gyro_updated || acc_updated || magn_updated || baro_updated) && !hil_enabled) {
|
||||
if ((gyro_updated || acc_updated || magn_updated || baro_updated) && publishing) {
|
||||
/* Values changed, publish */
|
||||
orb_publish(ORB_ID(sensor_combined), sensor_pub, &raw);
|
||||
}
|
||||
|
||||
@@ -234,7 +234,7 @@ unsigned char romfs_img[] = {
|
||||
0x69, 0x73, 0x61, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x46, 0x41, 0x49, 0x4c,
|
||||
0x45, 0x44, 0x2e, 0x22, 0x0a, 0x23, 0x09, 0x72, 0x65, 0x62, 0x6f, 0x6f,
|
||||
0x74, 0x0a, 0x23, 0x66, 0x69, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x0e, 0xf2,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xcd, 0x52, 0xce, 0xe0, 0xfc,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xcc, 0x52, 0xce, 0xe0, 0xfd,
|
||||
0x72, 0x63, 0x2e, 0x73, 0x74, 0x61, 0x6e, 0x64, 0x61, 0x6c, 0x6f, 0x6e,
|
||||
0x65, 0x00, 0x00, 0x00, 0x23, 0x21, 0x6e, 0x73, 0x68, 0x0a, 0x23, 0x0a,
|
||||
0x23, 0x20, 0x46, 0x6c, 0x69, 0x67, 0x68, 0x74, 0x20, 0x73, 0x74, 0x61,
|
||||
@@ -248,76 +248,76 @@ unsigned char romfs_img[] = {
|
||||
0x34, 0x46, 0x4d, 0x55, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x75, 0x70,
|
||||
0x2e, 0x2e, 0x2e, 0x22, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74,
|
||||
0x61, 0x72, 0x74, 0x20, 0x74, 0x68, 0x65, 0x20, 0x4f, 0x52, 0x42, 0x0a,
|
||||
0x23, 0x0a, 0x23, 0x75, 0x6f, 0x72, 0x62, 0x20, 0x73, 0x74, 0x61, 0x72,
|
||||
0x74, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72, 0x74,
|
||||
0x20, 0x74, 0x68, 0x65, 0x20, 0x73, 0x65, 0x6e, 0x73, 0x6f, 0x72, 0x73,
|
||||
0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x73, 0x68, 0x20, 0x2f, 0x65, 0x74, 0x63,
|
||||
0x2f, 0x69, 0x6e, 0x69, 0x74, 0x2e, 0x64, 0x2f, 0x72, 0x63, 0x2e, 0x73,
|
||||
0x65, 0x6e, 0x73, 0x6f, 0x72, 0x73, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20,
|
||||
0x53, 0x74, 0x61, 0x72, 0x74, 0x20, 0x4d, 0x41, 0x56, 0x4c, 0x69, 0x6e,
|
||||
0x6b, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x6d, 0x61, 0x76, 0x6c, 0x69, 0x6e,
|
||||
0x6b, 0x20, 0x2d, 0x64, 0x20, 0x2f, 0x64, 0x65, 0x76, 0x2f, 0x74, 0x74,
|
||||
0x79, 0x53, 0x30, 0x20, 0x2d, 0x62, 0x20, 0x35, 0x37, 0x36, 0x30, 0x30,
|
||||
0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72,
|
||||
0x74, 0x20, 0x74, 0x68, 0x65, 0x20, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e,
|
||||
0x64, 0x65, 0x72, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58,
|
||||
0x20, 0x74, 0x68, 0x69, 0x73, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64,
|
||||
0x20, 0x62, 0x65, 0x20, 0x27, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64,
|
||||
0x65, 0x72, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x27, 0x2e, 0x0a, 0x23,
|
||||
0x0a, 0x23, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x65, 0x72, 0x20,
|
||||
0x23, 0x0a, 0x75, 0x6f, 0x72, 0x62, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74,
|
||||
0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72, 0x74, 0x20,
|
||||
0x74, 0x68, 0x65, 0x20, 0x73, 0x65, 0x6e, 0x73, 0x6f, 0x72, 0x73, 0x2e,
|
||||
0x0a, 0x23, 0x0a, 0x23, 0x73, 0x68, 0x20, 0x2f, 0x65, 0x74, 0x63, 0x2f,
|
||||
0x69, 0x6e, 0x69, 0x74, 0x2e, 0x64, 0x2f, 0x72, 0x63, 0x2e, 0x73, 0x65,
|
||||
0x6e, 0x73, 0x6f, 0x72, 0x73, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53,
|
||||
0x74, 0x61, 0x72, 0x74, 0x20, 0x4d, 0x41, 0x56, 0x4c, 0x69, 0x6e, 0x6b,
|
||||
0x0a, 0x23, 0x0a, 0x23, 0x20, 0x6d, 0x61, 0x76, 0x6c, 0x69, 0x6e, 0x6b,
|
||||
0x20, 0x2d, 0x64, 0x20, 0x2f, 0x64, 0x65, 0x76, 0x2f, 0x74, 0x74, 0x79,
|
||||
0x53, 0x30, 0x20, 0x2d, 0x62, 0x20, 0x35, 0x37, 0x36, 0x30, 0x30, 0x20,
|
||||
0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72, 0x74,
|
||||
0x20, 0x74, 0x68, 0x65, 0x20, 0x61, 0x74, 0x74, 0x69, 0x74, 0x75, 0x64,
|
||||
0x65, 0x20, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61, 0x74, 0x6f, 0x72, 0x0a,
|
||||
0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68, 0x69, 0x73,
|
||||
0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x62, 0x65, 0x20, 0x27,
|
||||
0x3c, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x3e, 0x20, 0x73, 0x74,
|
||||
0x61, 0x72, 0x74, 0x27, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x61, 0x74, 0x74,
|
||||
0x69, 0x74, 0x75, 0x64, 0x65, 0x5f, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61,
|
||||
0x74, 0x6f, 0x72, 0x5f, 0x62, 0x6d, 0x20, 0x26, 0x0a, 0x23, 0x70, 0x6f,
|
||||
0x73, 0x69, 0x74, 0x69, 0x6f, 0x6e, 0x5f, 0x65, 0x73, 0x74, 0x69, 0x6d,
|
||||
0x61, 0x74, 0x6f, 0x72, 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20,
|
||||
0x53, 0x74, 0x61, 0x72, 0x74, 0x20, 0x74, 0x68, 0x65, 0x20, 0x66, 0x69,
|
||||
0x78, 0x65, 0x64, 0x2d, 0x77, 0x69, 0x6e, 0x67, 0x20, 0x63, 0x6f, 0x6e,
|
||||
0x74, 0x72, 0x6f, 0x6c, 0x6c, 0x65, 0x72, 0x2e, 0x0a, 0x23, 0x0a, 0x23,
|
||||
0x20, 0x58, 0x58, 0x58, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20,
|
||||
0x74, 0x68, 0x69, 0x73, 0x20, 0x62, 0x65, 0x20, 0x6c, 0x6f, 0x6f, 0x6b,
|
||||
0x69, 0x6e, 0x67, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x63, 0x6f, 0x6e, 0x66,
|
||||
0x69, 0x67, 0x75, 0x72, 0x61, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x74, 0x6f,
|
||||
0x20, 0x64, 0x65, 0x63, 0x69, 0x64, 0x65, 0x0a, 0x23, 0x20, 0x77, 0x68,
|
||||
0x65, 0x74, 0x68, 0x65, 0x72, 0x20, 0x74, 0x68, 0x65, 0x20, 0x62, 0x6f,
|
||||
0x61, 0x72, 0x64, 0x20, 0x69, 0x73, 0x20, 0x63, 0x6f, 0x6e, 0x66, 0x69,
|
||||
0x67, 0x75, 0x72, 0x65, 0x64, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x66, 0x69,
|
||||
0x78, 0x65, 0x64, 0x2d, 0x77, 0x69, 0x6e, 0x67, 0x20, 0x75, 0x73, 0x65,
|
||||
0x3f, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68,
|
||||
0x69, 0x73, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x62, 0x65,
|
||||
0x20, 0x27, 0x66, 0x69, 0x78, 0x65, 0x64, 0x77, 0x69, 0x6e, 0x67, 0x5f,
|
||||
0x63, 0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c, 0x20, 0x73, 0x74, 0x61, 0x72,
|
||||
0x74, 0x27, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x66, 0x69, 0x78, 0x65, 0x64,
|
||||
0x77, 0x69, 0x6e, 0x67, 0x5f, 0x63, 0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c,
|
||||
0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x43, 0x6f, 0x6e, 0x66,
|
||||
0x69, 0x67, 0x75, 0x72, 0x65, 0x20, 0x46, 0x4d, 0x55, 0x20, 0x66, 0x6f,
|
||||
0x72, 0x20, 0x73, 0x74, 0x61, 0x6e, 0x64, 0x61, 0x6c, 0x6f, 0x6e, 0x65,
|
||||
0x20, 0x6d, 0x6f, 0x64, 0x65, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58,
|
||||
0x58, 0x20, 0x61, 0x72, 0x67, 0x75, 0x6d, 0x65, 0x6e, 0x74, 0x73, 0x3f,
|
||||
0x0a, 0x23, 0x0a, 0x23, 0x70, 0x78, 0x34, 0x66, 0x6d, 0x75, 0x20, 0x73,
|
||||
0x74, 0x61, 0x72, 0x74, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74,
|
||||
0x61, 0x72, 0x74, 0x20, 0x6c, 0x6f, 0x6f, 0x6b, 0x69, 0x6e, 0x67, 0x20,
|
||||
0x66, 0x6f, 0x72, 0x20, 0x61, 0x20, 0x47, 0x50, 0x53, 0x2e, 0x0a, 0x23,
|
||||
0x20, 0x74, 0x68, 0x65, 0x20, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64,
|
||||
0x65, 0x72, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20,
|
||||
0x74, 0x68, 0x69, 0x73, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20,
|
||||
0x62, 0x65, 0x20, 0x27, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x65,
|
||||
0x72, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x27, 0x2e, 0x0a, 0x23, 0x0a,
|
||||
0x23, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x65, 0x72, 0x20, 0x26,
|
||||
0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72, 0x74, 0x20,
|
||||
0x74, 0x68, 0x65, 0x20, 0x61, 0x74, 0x74, 0x69, 0x74, 0x75, 0x64, 0x65,
|
||||
0x20, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61, 0x74, 0x6f, 0x72, 0x0a, 0x23,
|
||||
0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68, 0x69, 0x73, 0x20,
|
||||
0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x6e, 0x6f, 0x74, 0x20, 0x6e,
|
||||
0x65, 0x65, 0x64, 0x20, 0x74, 0x6f, 0x20, 0x62, 0x65, 0x20, 0x62, 0x61,
|
||||
0x63, 0x6b, 0x67, 0x72, 0x6f, 0x75, 0x6e, 0x64, 0x65, 0x64, 0x0a, 0x23,
|
||||
0x0a, 0x23, 0x67, 0x70, 0x73, 0x20, 0x2d, 0x64, 0x20, 0x2f, 0x64, 0x65,
|
||||
0x76, 0x2f, 0x74, 0x74, 0x79, 0x53, 0x33, 0x20, 0x2d, 0x6d, 0x20, 0x61,
|
||||
0x6c, 0x6c, 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74,
|
||||
0x61, 0x72, 0x74, 0x20, 0x6c, 0x6f, 0x67, 0x67, 0x69, 0x6e, 0x67, 0x20,
|
||||
0x74, 0x6f, 0x20, 0x6d, 0x69, 0x63, 0x72, 0x6f, 0x53, 0x44, 0x20, 0x69,
|
||||
0x66, 0x20, 0x77, 0x65, 0x20, 0x63, 0x61, 0x6e, 0x0a, 0x23, 0x0a, 0x73,
|
||||
0x68, 0x20, 0x2f, 0x65, 0x74, 0x63, 0x2f, 0x69, 0x6e, 0x69, 0x74, 0x2e,
|
||||
0x64, 0x2f, 0x72, 0x63, 0x2e, 0x6c, 0x6f, 0x67, 0x67, 0x69, 0x6e, 0x67,
|
||||
0x0a, 0x0a, 0x65, 0x63, 0x68, 0x6f, 0x20, 0x22, 0x5b, 0x69, 0x6e, 0x69,
|
||||
0x74, 0x5d, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x75, 0x70, 0x20, 0x64,
|
||||
0x6f, 0x6e, 0x65, 0x22, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0a,
|
||||
0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x62, 0x65, 0x20, 0x27, 0x3c,
|
||||
0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x3e, 0x20, 0x73, 0x74, 0x61,
|
||||
0x72, 0x74, 0x27, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x61, 0x74, 0x74, 0x69,
|
||||
0x74, 0x75, 0x64, 0x65, 0x5f, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61, 0x74,
|
||||
0x6f, 0x72, 0x5f, 0x62, 0x6d, 0x20, 0x26, 0x0a, 0x23, 0x70, 0x6f, 0x73,
|
||||
0x69, 0x74, 0x69, 0x6f, 0x6e, 0x5f, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61,
|
||||
0x74, 0x6f, 0x72, 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53,
|
||||
0x74, 0x61, 0x72, 0x74, 0x20, 0x74, 0x68, 0x65, 0x20, 0x66, 0x69, 0x78,
|
||||
0x65, 0x64, 0x2d, 0x77, 0x69, 0x6e, 0x67, 0x20, 0x63, 0x6f, 0x6e, 0x74,
|
||||
0x72, 0x6f, 0x6c, 0x6c, 0x65, 0x72, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x20,
|
||||
0x58, 0x58, 0x58, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x74,
|
||||
0x68, 0x69, 0x73, 0x20, 0x62, 0x65, 0x20, 0x6c, 0x6f, 0x6f, 0x6b, 0x69,
|
||||
0x6e, 0x67, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x63, 0x6f, 0x6e, 0x66, 0x69,
|
||||
0x67, 0x75, 0x72, 0x61, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x74, 0x6f, 0x20,
|
||||
0x64, 0x65, 0x63, 0x69, 0x64, 0x65, 0x0a, 0x23, 0x20, 0x77, 0x68, 0x65,
|
||||
0x74, 0x68, 0x65, 0x72, 0x20, 0x74, 0x68, 0x65, 0x20, 0x62, 0x6f, 0x61,
|
||||
0x72, 0x64, 0x20, 0x69, 0x73, 0x20, 0x63, 0x6f, 0x6e, 0x66, 0x69, 0x67,
|
||||
0x75, 0x72, 0x65, 0x64, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x66, 0x69, 0x78,
|
||||
0x65, 0x64, 0x2d, 0x77, 0x69, 0x6e, 0x67, 0x20, 0x75, 0x73, 0x65, 0x3f,
|
||||
0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68, 0x69,
|
||||
0x73, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x62, 0x65, 0x20,
|
||||
0x27, 0x66, 0x69, 0x78, 0x65, 0x64, 0x77, 0x69, 0x6e, 0x67, 0x5f, 0x63,
|
||||
0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74,
|
||||
0x27, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x66, 0x69, 0x78, 0x65, 0x64, 0x77,
|
||||
0x69, 0x6e, 0x67, 0x5f, 0x63, 0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c, 0x20,
|
||||
0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x43, 0x6f, 0x6e, 0x66, 0x69,
|
||||
0x67, 0x75, 0x72, 0x65, 0x20, 0x46, 0x4d, 0x55, 0x20, 0x66, 0x6f, 0x72,
|
||||
0x20, 0x73, 0x74, 0x61, 0x6e, 0x64, 0x61, 0x6c, 0x6f, 0x6e, 0x65, 0x20,
|
||||
0x6d, 0x6f, 0x64, 0x65, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58,
|
||||
0x20, 0x61, 0x72, 0x67, 0x75, 0x6d, 0x65, 0x6e, 0x74, 0x73, 0x3f, 0x0a,
|
||||
0x23, 0x0a, 0x23, 0x70, 0x78, 0x34, 0x66, 0x6d, 0x75, 0x20, 0x73, 0x74,
|
||||
0x61, 0x72, 0x74, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61,
|
||||
0x72, 0x74, 0x20, 0x6c, 0x6f, 0x6f, 0x6b, 0x69, 0x6e, 0x67, 0x20, 0x66,
|
||||
0x6f, 0x72, 0x20, 0x61, 0x20, 0x47, 0x50, 0x53, 0x2e, 0x0a, 0x23, 0x0a,
|
||||
0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68, 0x69, 0x73, 0x20, 0x73,
|
||||
0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x6e, 0x6f, 0x74, 0x20, 0x6e, 0x65,
|
||||
0x65, 0x64, 0x20, 0x74, 0x6f, 0x20, 0x62, 0x65, 0x20, 0x62, 0x61, 0x63,
|
||||
0x6b, 0x67, 0x72, 0x6f, 0x75, 0x6e, 0x64, 0x65, 0x64, 0x0a, 0x23, 0x0a,
|
||||
0x23, 0x67, 0x70, 0x73, 0x20, 0x2d, 0x64, 0x20, 0x2f, 0x64, 0x65, 0x76,
|
||||
0x2f, 0x74, 0x74, 0x79, 0x53, 0x33, 0x20, 0x2d, 0x6d, 0x20, 0x61, 0x6c,
|
||||
0x6c, 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61,
|
||||
0x72, 0x74, 0x20, 0x6c, 0x6f, 0x67, 0x67, 0x69, 0x6e, 0x67, 0x20, 0x74,
|
||||
0x6f, 0x20, 0x6d, 0x69, 0x63, 0x72, 0x6f, 0x53, 0x44, 0x20, 0x69, 0x66,
|
||||
0x20, 0x77, 0x65, 0x20, 0x63, 0x61, 0x6e, 0x0a, 0x23, 0x0a, 0x73, 0x68,
|
||||
0x20, 0x2f, 0x65, 0x74, 0x63, 0x2f, 0x69, 0x6e, 0x69, 0x74, 0x2e, 0x64,
|
||||
0x2f, 0x72, 0x63, 0x2e, 0x6c, 0x6f, 0x67, 0x67, 0x69, 0x6e, 0x67, 0x0a,
|
||||
0x0a, 0x65, 0x63, 0x68, 0x6f, 0x20, 0x22, 0x5b, 0x69, 0x6e, 0x69, 0x74,
|
||||
0x5d, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x75, 0x70, 0x20, 0x64, 0x6f,
|
||||
0x6e, 0x65, 0x22, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0a,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x76, 0x8d, 0x9c, 0xa3, 0x80,
|
||||
0x72, 0x63, 0x53, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x23, 0x21, 0x6e, 0x73, 0x68, 0x0a, 0x23, 0x0a,
|
||||
|
||||
@@ -204,7 +204,7 @@ read_values(int16_t *data)
|
||||
int hmc_status = hmc5883l_read_reg(ADDR_STATUS);
|
||||
if (hmc_status < 0)
|
||||
{
|
||||
if (ret == ETIMEDOUT) hmc5883l_reset();
|
||||
if (hmc_status == ETIMEDOUT) hmc5883l_reset();
|
||||
ret = hmc_status;
|
||||
}
|
||||
else
|
||||
|
||||
@@ -123,6 +123,16 @@ static FAR struct {
|
||||
|
||||
static int ms5611_read_prom(void);
|
||||
|
||||
int ms5611_reset()
|
||||
{
|
||||
int ret;
|
||||
printf("[ms5611 drv] Resettet I2C2 BUS\n");
|
||||
up_i2cuninitialize(ms5611_dev.i2c);
|
||||
ms5611_dev.i2c = up_i2cinitialize(2);
|
||||
I2C_SETFREQUENCY(ms5611_dev.i2c, 400000);
|
||||
return ret;
|
||||
}
|
||||
|
||||
static bool
|
||||
read_values(float *data)
|
||||
{
|
||||
@@ -279,6 +289,7 @@ read_values(float *data)
|
||||
else
|
||||
{
|
||||
errno = -ret;
|
||||
if (errno == ETIMEDOUT || ret == ETIMEDOUT) ms5611_reset();
|
||||
return ret;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user