Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-05 06:03:02 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

Kicked out mix_and_link, deleted old MPU driver, disabled (still needed for reference) old HMC and MS5611 drivers. Removed driver init from up_nsh.c. Reworked fixedwing_control to be closer to up-to-date api, still more clean up needed. Fixed a bug that limited the motor thrust for multirotor control

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-24 11:40:16 +02:00
parent a69c55f671
commit a1b99a3f03
11 changed files with 86 additions and 1025 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/fixedwing_control/fixedwing_control.c
+67 -172
View File
@@ -52,14 +52,15 @@
#include <arch/board/board.h> #include <arch/board/board.h>
#include <drivers/drv_pwm_output.h> #include <drivers/drv_pwm_output.h>
#include <nuttx/spi.h> #include <nuttx/spi.h>
#include "../mix_and_link/mix_and_link.h" //TODO: add to Makefile
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/rc_channels.h> #include <uORB/topics/rc_channels.h>
#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/fixedwing_control.h> #include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
__EXPORT int fixedwing_control_main(int argc, char *argv[]); __EXPORT int fixedwing_control_main(int argc, char *argv[]);
@@ -710,19 +711,38 @@ int fixedwing_control_main(int argc, char *argv[])
/* welcome user */ /* welcome user */
printf("[fixedwing control] started\n"); printf("[fixedwing control] started\n");
/* Set up to publish fixed wing control messages */ /* output structs */
struct fixedwing_control_s control; struct actuator_controls_s actuators;
orb_advert_t fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control); struct actuator_armed_s armed;
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
/* publish actuator controls */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
actuators.control[i] = 0.0f;
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
armed.armed = false;
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
/* Subscribe to global position, attitude and rc */ /* Subscribe to global position, attitude and rc */
struct vehicle_global_position_s global_pos; struct vehicle_global_position_s global_pos;
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
struct vehicle_attitude_s att;
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; struct vehicle_global_position_setpoint_s global_setpoint;
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
/* declare and safely initialize all structs */
struct vehicle_status_s state;
memset(&state, 0, sizeof(state));
struct vehicle_attitude_s att;
memset(&att_sp, 0, sizeof(att_sp));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
/* subscribe to attitude, motor setpoints and system state */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
/* Mainloop setup */ /* Mainloop setup */
unsigned int loopcounter = 0; unsigned int loopcounter = 0;
@@ -734,44 +754,6 @@ int fixedwing_control_main(int argc, char *argv[])
/* Servo setup */ /* Servo setup */
int fd;
servo_position_t data[PWM_OUTPUT_MAX_CHANNELS];
fd = open("/dev/pwm_servo", O_RDWR);
if (fd < 0) {
printf("[fixedwing control] Failed opening /dev/pwm_servo\n");
}
ioctl(fd, PWM_SERVO_ARM, 0);
int16_t buffer_rc[3];
int16_t buffer_servo[3];
mixer_data_t mixer_buffer;
mixer_buffer.input = buffer_rc;
mixer_buffer.output = buffer_servo;
mixer_conf_t mixers[3];
mixers[0].source = PITCH;
mixers[0].nr_actuators = 2;
mixers[0].dest[0] = AIL_1;
mixers[0].dest[1] = AIL_2;
mixers[0].dual_rate[0] = 1;
mixers[0].dual_rate[1] = 1;
mixers[1].source = ROLL;
mixers[1].nr_actuators = 2;
mixers[1].dest[0] = AIL_1;
mixers[1].dest[1] = AIL_2;
mixers[1].dual_rate[0] = 1;
mixers[1].dual_rate[1] = -1;
mixers[2].source = THROTTLE;
mixers[2].nr_actuators = 1;
mixers[2].dest[0] = MOT;
mixers[2].dual_rate[0] = 1;
/* /*
* Main control, navigation and servo routine * Main control, navigation and servo routine
*/ */
@@ -786,8 +768,15 @@ int fixedwing_control_main(int argc, char *argv[])
// XXX add error checking // XXX add error checking
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); 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_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); /* get a local copy of system state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
/* get a local copy of attitude setpoint */
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
/* scaling factors are defined by the data from the APM Planner /* scaling factors are defined by the data from the APM Planner
* TODO: ifdef for other parameters (HIL/Real world switch) * TODO: ifdef for other parameters (HIL/Real world switch)
@@ -810,20 +799,19 @@ int fixedwing_control_main(int argc, char *argv[])
plane_data.yawspeed = att.yawspeed; plane_data.yawspeed = att.yawspeed;
/* parameter values */ /* parameter values */
get_parameters(&plane_data); if (loopcounter % 20 == 0) {
get_parameters(&plane_data);
}
/* Attitude control part */ /* Attitude control part */
if (verbose && loopcounter % 20 == 0) { if (verbose && loopcounter % 20 == 0) {
/******************************** DEBUG OUTPUT ************************************************************/ /******************************** DEBUG OUTPUT ************************************************************/
printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)plane_data.Kp_att, (int)plane_data.Ki_att, 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_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.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, (int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z);
(int)global_data_parameter_storage->pm.param_values[PARAM_WPLON],
(int)global_data_parameter_storage->pm.param_values[PARAM_WPLAT],
(int)global_data_parameter_storage->pm.param_values[PARAM_WPALT]);
printf("PITCH SETPOINT: %i\n", (int)(100.0f*plane_data.pitch_setpoint)); printf("PITCH SETPOINT: %i\n", (int)(100.0f*plane_data.pitch_setpoint));
printf("ROLL SETPOINT: %i\n", (int)(100.0f*plane_data.roll_setpoint)); printf("ROLL SETPOINT: %i\n", (int)(100.0f*plane_data.roll_setpoint));
@@ -840,20 +828,13 @@ int fixedwing_control_main(int argc, char *argv[])
// printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n", // 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)); // (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", printf("\nCalculated outputs: \n R: %8.4f\n P: %8.4f\n Y: %8.4f\n T: %8.4f \n",
(int)(10000.0f * control_outputs.roll_ailerons), (int)(10000.0f * control_outputs.pitch_elevator), att_sp.roll_body, att_sp.pitch_body,
(int)(10000.0f * control_outputs.yaw_rudder), (int)(10000.0f * control_outputs.throttle)); att_sp.yaw_body, att_sp.thrust);
/************************************************************************************************************/ /************************************************************************************************************/
} }
/*
* Computation section
*
* The function calls to compute the required control values happen
* in this section.
*/
/* Set plane mode */ /* Set plane mode */
set_plane_mode(); set_plane_mode();
@@ -864,123 +845,37 @@ int fixedwing_control_main(int argc, char *argv[])
/* Calculate the body frame angles of the aircraft */ /* Calculate the body frame angles of the aircraft */
//calc_bodyframe_angles(plane_data.roll,plane_data.pitch,plane_data.yaw); //calc_bodyframe_angles(plane_data.roll,plane_data.pitch,plane_data.yaw);
/* Calculate the output values */ // if (manual.override_mode_switch < XXX) {
control_outputs.roll_ailerons = calc_roll_ail();
control_outputs.pitch_elevator = calc_pitch_elev();
//control_outputs.yaw_rudder = calc_yaw_rudder();
control_outputs.throttle = calc_throttle();
if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // if we're flying in automated mode
/*
* TAKEOFF hack for HIL
*/
if (plane_data.mode == TAKEOFF) {
control.attitude_control_output[ROLL] = 0;
control.attitude_control_output[PITCH] = 5000;
control.attitude_control_output[THROTTLE] = 10000;
//global_data_fixedwing_control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
}
if (plane_data.mode == CRUISE) {
control.attitude_control_output[ROLL] = control_outputs.roll_ailerons;
control.attitude_control_output[PITCH] = control_outputs.pitch_elevator;
control.attitude_control_output[THROTTLE] = control_outputs.throttle;
//control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
}
control.counter++;
control.timestamp = hrt_absolute_time();
}
/* Navigation part */ /* Navigation part */
// Get GPS Waypoint // Get Waypoint
// if(global_data_wait(&global_data_position_setpoint->access_conf) == 0) // XXX FIXME
// {
// plane_data.wp_x = global_data_position_setpoint->x;
// plane_data.wp_y = global_data_position_setpoint->y;
// plane_data.wp_z = global_data_position_setpoint->z;
// }
// global_data_unlock(&global_data_position_setpoint->access_conf);
if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // AUTO mode //else if (manual.override_mode_switch > MANUAL) { // AUTO mode
// AUTO/HYBRID switch
/* calculate setpoint based on current position and waypoint */
att_sp.roll_body = calc_roll_setpoint();
att_sp.pitch_body = calc_pitch_setpoint();
att_sp.thrust = calc_throttle_setpoint();
if (rc.chan[rc.function[OVERRIDE]].scale < AUTO) { /* calculate the control outputs based on roll / pitch / yaw setpoints */
plane_data.roll_setpoint = calc_roll_setpoint(); actuators.control[0] = calc_roll_ail();
plane_data.pitch_setpoint = calc_pitch_setpoint(); actuators.control[1] = calc_pitch_elev();
plane_data.throttle_setpoint = calc_throttle_setpoint(); actuators.control[2] = calc_yaw_rudder(att.yaw);
actuators.control[3] = calc_throttle();
} else { /* publish attitude setpoint (for MAVLink) */
plane_data.roll_setpoint = rc.chan[rc.function[ROLL]].scale / 200; orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
plane_data.pitch_setpoint = rc.chan[rc.function[PITCH]].scale / 200;
plane_data.throttle_setpoint = rc.chan[rc.function[THROTTLE]].scale / 200;
}
//control_outputs.yaw_rudder = calc_yaw_rudder(plane_data.hdg); /* publish actuator setpoints (for mixer) */
} else { /* arming state depends on commander arming state */
control.attitude_control_output[ROLL] = rc.chan[rc.function[ROLL]].scale/10000; armed.armed = state.flag_system_armed;
control.attitude_control_output[PITCH] = rc.chan[rc.function[PITCH]].scale/10000; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
control.attitude_control_output[THROTTLE] = rc.chan[rc.function[THROTTLE]].scale/10000; orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
// since we don't have a yaw rudder
//control->attitude_control_output[3] = global_data_rc_channels->chan[YAW].scale;
control.counter++;
control.timestamp = hrt_absolute_time();
}
/* publish the control data */
orb_publish(ORB_ID(fixedwing_control), fixedwing_control_pub, &control);
/* Servo part */
buffer_rc[ROLL] = (int16_t)(10000*control.attitude_control_output[ROLL]);
buffer_rc[PITCH] = (int16_t)(10000*control.attitude_control_output[PITCH]);
buffer_rc[THROTTLE] = (int16_t)(10000*control.attitude_control_output[THROTTLE]);
//mix_and_link(mixers, 3, 2, &mixer_buffer);
// Scaling and saturation of servo outputs happens here
data[AIL_1] = buffer_servo[AIL_1] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
+ global_data_parameter_storage->pm.param_values[PARAM_SERVO1_TRIM];
if (data[AIL_1] > SERVO_MAX)
data[AIL_1] = SERVO_MAX;
if (data[AIL_1] < SERVO_MIN)
data[AIL_1] = SERVO_MIN;
data[AIL_2] = buffer_servo[AIL_2] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
+ global_data_parameter_storage->pm.param_values[PARAM_SERVO2_TRIM];
if (data[AIL_2] > SERVO_MAX)
data[AIL_2] = SERVO_MAX;
if (data[AIL_2] < SERVO_MIN)
data[AIL_2] = SERVO_MIN;
data[MOT] = buffer_servo[MOT] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
+ global_data_parameter_storage->pm.param_values[PARAM_SERVO3_TRIM];
if (data[MOT] > SERVO_MAX)
data[MOT] = SERVO_MAX;
if (data[MOT] < SERVO_MIN)
data[MOT] = SERVO_MIN;
int result = write(fd, &data, sizeof(data));
if (result != sizeof(data)) {
if (failcounter < 10 || failcounter % 20 == 0) {
printf("[fixedwing_control] failed writing servo outputs\n");
}
failcounter++;
}
loopcounter++; loopcounter++;
apps/mix_and_link/Makefile
-38
View File
@@ -1,38 +0,0 @@
############################################################################
#
# Copyright (C) 2012 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.
#
############################################################################
#
# Makefile to build the mix-and-link functions
#
include $(APPDIR)/mk/app.mk
apps/mix_and_link/mix_and_link.c
-81
View File
@@ -1,81 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Nils Wenzler <wenzlern@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 Mixing / linking of RC channels
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdint.h>
#include "mix_and_link.h"
/*
* Library function that mixes the abstract functions like ROLL, PITCH, THROTTLE
* to one or several actuators.
* @param *mixers is a pointer to the configuration struct, declared in mix_and_link.h
* @param nr_mixers is a integer that denotes the number of mixers that should be processed
* @param nr_actuators is a integer that denotes the max number of actuators that get linked to one source
* @param *data is a pointer to the mixer data struct, declared in mix_and_link.h
*/
void mix_and_link(const mixer_conf_t *mixers, uint8_t nr_mixers, uint8_t nr_actuators, mixer_data_t *data)
{
/* Reset the Output Array */
uint8_t i, j;
for (i = 0; i < nr_actuators; i++) {
data->output[i] = 0;
}
/* Loop throug the given mixers */
for (i = 0; i < nr_mixers; i++) {
/* The actual mixing */
for (j = 0; j < mixers[i].nr_actuators; j++) {
data->output[mixers[i].dest[j]] += (int16_t)(data->input[mixers[i].source]
* mixers[i].dual_rate[j]);
/* Saturate to +-10000 */
if (data->output[mixers[i].dest[j]] > 10000)
data->output[mixers[i].dest[j]] = 10000;
else if (data->output[mixers[i].dest[j]] < -10000)
data->output[mixers[i].dest[j]] = -10000;
}
}
}
apps/mix_and_link/mix_and_link.h
-115
View File
@@ -1,115 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Nils Wenzler <wenzlern@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.
*
****************************************************************************/
#ifndef _MIX_AND_LINK_H_
#define _MIX_AND_LINK_H_
#include <nuttx/config.h>
#include <unistd.h>
#include <stdint.h>
/**< The maximum number of actuators that get linked to one abstract function*/
#define NR_MAX_ACTUATORS 2
/**
* The struct mixer_conf_t contains all the necessary information for one mixer.
* Make an array of type mixer_conf_t for several mix functions.
*/
typedef struct {
uint8_t source; /**< source RC channel index 0..n */
uint8_t nr_actuators; /**< number of actuators to output to */
uint8_t dest[NR_MAX_ACTUATORS]; /**< Which actuators to output to */
float dual_rate[NR_MAX_ACTUATORS]; /**< Direction and rate of mixing. Range [-1,1] */
} mixer_conf_t; /**< Setup for one mixer */
/*
* The struct mixer_data contains two int16_t arrays with the abstract source functions
* (Throttle, Roll,...) in the input, and an empty array output of the desired
* size for the results.
*/
typedef struct {
int16_t *input;
int16_t *output;
} mixer_data_t;
/****************************************************************************
* Public Functions
****************************************************************************/
/*
* Mixes the functions to the actuators.
* e.g. Throttle, Roll,...
*
* Each mixer_conf struct in the mixers array contains the source function (ex. THROTTLE), the number
* of actuators, an array with the desired actuators, Dual Rate determines which
* percentage of the source gets mixed to the destination and sets the direction.
* It's range is between -1 and 1.
*
* EXAMPLE: Deltamix for a wing plane:
*
* mixer_data_t mixer_buffer;
* mixer_buffer.input = buffer_rc;
* mixer_buffer.output = buffer_servo;
*
*
* mixer_conf_t mixers[3];
*
* mixers[0].source = PITCH;
* mixers[0].nr_actuators = 2;
* mixers[0].dest[0] = AIL_1;
* mixers[0].dest[1] = AIL_2;
* mixers[0].dual_rate[0] = 1;
* mixers[0].dual_rate[1] = 1;
*
* mixers[1].source = ROLL;
* mixers[1].nr_actuators = 2;
* mixers[1].dest[0] = AIL_1;
* mixers[1].dest[1] = AIL_2;
* mixers[1].dual_rate[0] = 1;
* mixers[1].dual_rate[0] = -1;
*
* mixers[2].source = THROTTLE;
* mixers[2].nr_actuators = 1;
* mixers[2].dest[0] = MOT;
* mixers[2].dual_rate[0] = 1;
*
* @param mixers pointer to the mixer struct array
* @param nr_mixers number of mixers in struct array
* @param nr_actuators number of actuators
* @param data pointer to the mixer_data struct.
*/
void mix_and_link(const mixer_conf_t *mixers, uint8_t nr_mixers, uint8_t nr_actuators, mixer_data_t *data);
#endif
apps/multirotor_att_control/multirotor_att_control_main.c
+2 -9
View File
@@ -96,8 +96,6 @@ mc_thread_main(int argc, char *argv[])
memset(&raw, 0, sizeof(raw)); memset(&raw, 0, sizeof(raw));
struct ardrone_motors_setpoint_s setpoint; struct ardrone_motors_setpoint_s setpoint;
memset(&setpoint, 0, sizeof(setpoint)); memset(&setpoint, 0, sizeof(setpoint));
struct actuator_controls_s actuator_controls;
memset(&actuator_controls, 0, sizeof(actuator_controls));
struct actuator_controls_s actuators; struct actuator_controls_s actuators;
struct actuator_armed_s armed; struct actuator_armed_s armed;
@@ -153,12 +151,7 @@ mc_thread_main(int argc, char *argv[])
att_sp.yaw_body = 0.0f; att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.1f; att_sp.thrust = 0.1f;
} else { } else {
if (state.state_machine == SYSTEM_STATE_MANUAL || if (state.flag_system_armed) {
state.state_machine == SYSTEM_STATE_GROUND_READY ||
state.state_machine == SYSTEM_STATE_STABILIZED ||
state.state_machine == SYSTEM_STATE_AUTO ||
state.state_machine == SYSTEM_STATE_MISSION_ABORT ||
state.state_machine == SYSTEM_STATE_EMCY_LANDING) {
att_sp.thrust = manual.throttle; att_sp.thrust = manual.throttle;
armed.armed = true; armed.armed = true;
@@ -173,7 +166,7 @@ mc_thread_main(int argc, char *argv[])
} }
multirotor_control_attitude(&att_sp, &att, &state, &actuator_controls, motor_test_mode); multirotor_control_attitude(&att_sp, &att, &state, &actuators, motor_test_mode);
/* publish the result */ /* publish the result */
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
nuttx/configs/px4fmu/include/drv_mpu6000.h
-92
View File
@@ -1,92 +0,0 @@
/*
* Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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.
*/
/*
* Driver for the Invense MPU-6000 gyroscope
*/
/* IMPORTANT NOTES:
*
* SPI max. clock frequency: 10 Mhz
* CS has to be high before transfer,
* go low right before transfer and
* go high again right after transfer
*
*/
#include <sys/ioctl.h>
#define _MPU6000BASE 0x7600
#define MPU6000C(_x) _IOC(_MPU6000BASE, _x)
/*
* Sets the sensor internal sampling rate, and if a buffer
* has been configured, the rate at which entries will be
* added to the buffer.
*/
#define MPU6000_SETRATE MPU6000C(1)
#define MPU6000_RATE_95HZ_LP_12_5HZ ((0<<7) | (0<<6) | (0<<5) | (0<<4))
/*
* Sets the sensor internal range.
*/
#define MPU6000_SETRANGE MPU6000C(2)
#define MPU6000_RANGE_250DPS (0<<4)
#define MPU6000_RATE_95HZ ((0<<6) | (0<<4))
/*
* Sets the address of a shared MPU6000_buffer
* structure that is maintained by the driver.
*
* If zero is passed as the address, disables
* the buffer updating.
*/
#define MPU6000_SETBUFFER MPU6000C(3)
struct MPU6000_buffer {
uint32_t size; /* number of entries in the samples[] array */
uint32_t next; /* the next entry that will be populated */
struct {
uint16_t x;
uint16_t y;
uint16_t z;
uint16_t roll;
uint16_t pitch;
uint16_t yaw;
} samples[];
};
extern int mpu6000_attach(struct spi_dev_s *spi, int spi_id);
nuttx/configs/px4fmu/nsh/appconfig
-2
View File
@@ -71,12 +71,10 @@ CONFIGURED_APPS += gps
CONFIGURED_APPS += commander CONFIGURED_APPS += commander
#CONFIGURED_APPS += sdlog #CONFIGURED_APPS += sdlog
CONFIGURED_APPS += sensors CONFIGURED_APPS += sensors
CONFIGURED_APPS += ardrone_control
CONFIGURED_APPS += ardrone_interface CONFIGURED_APPS += ardrone_interface
CONFIGURED_APPS += multirotor_att_control CONFIGURED_APPS += multirotor_att_control
CONFIGURED_APPS += px4/attitude_estimator_bm CONFIGURED_APPS += px4/attitude_estimator_bm
CONFIGURED_APPS += fixedwing_control CONFIGURED_APPS += fixedwing_control
CONFIGURED_APPS += mix_and_link
CONFIGURED_APPS += position_estimator CONFIGURED_APPS += position_estimator
CONFIGURED_APPS += attitude_estimator_ekf CONFIGURED_APPS += attitude_estimator_ekf
nuttx/configs/px4fmu/src/Makefile
+2 -2
View File
@@ -42,9 +42,9 @@ AOBJS = $(ASRCS:.S=$(OBJEXT))
CSRCS = up_boot.c up_leds.c up_spi.c up_hrt.c \ CSRCS = up_boot.c up_leds.c up_spi.c up_hrt.c \
drv_gpio.c drv_bma180.c drv_l3gd20.c \ drv_gpio.c drv_bma180.c drv_l3gd20.c \
drv_led.c drv_hmc5833l.c drv_ms5611.c drv_eeprom.c \ drv_led.c drv_eeprom.c \
drv_tone_alarm.c up_pwm_servo.c up_usbdev.c \ drv_tone_alarm.c up_pwm_servo.c up_usbdev.c \
up_cpuload.c drv_mpu6000.c up_cpuload.c
ifeq ($(CONFIG_NSH_ARCHINIT),y) ifeq ($(CONFIG_NSH_ARCHINIT),y)
CSRCS += up_nsh.c CSRCS += up_nsh.c
nuttx/configs/px4fmu/src/drv_mpu6000.c
-433
View File
@@ -1,433 +0,0 @@
/*
* Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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.
*/
/*
* Driver for the ST mpu6000 MEMS gyroscope
*/
#include <nuttx/config.h>
#include <stdint.h>
#include <stdbool.h>
#include <debug.h>
#include <errno.h>
#include <unistd.h>
#include <stdio.h>
#include <nuttx/spi.h>
#include <arch/board/board.h>
#include <nuttx/arch.h>
#include "chip.h"
#include "stm32_internal.h"
#include "px4fmu-internal.h"
#include <arch/board/drv_mpu6000.h>
#define DIR_READ (0x80)
#define DIR_WRITE (0<<7)
#define ADDR_INCREMENT (1<<6)
#define WHO_I_AM 0xD4
// MPU 6000 registers
#define MPUREG_WHOAMI 0x75 //
#define MPUREG_SMPLRT_DIV 0x19 //
#define MPUREG_CONFIG 0x1A //
#define MPUREG_GYRO_CONFIG 0x1B
#define MPUREG_ACCEL_CONFIG 0x1C
#define MPUREG_FIFO_EN 0x23
#define MPUREG_INT_PIN_CFG 0x37
#define MPUREG_INT_ENABLE 0x38
#define MPUREG_INT_STATUS 0x3A
#define MPUREG_ACCEL_XOUT_H 0x3B //
#define MPUREG_ACCEL_XOUT_L 0x3C //
#define MPUREG_ACCEL_YOUT_H 0x3D //
#define MPUREG_ACCEL_YOUT_L 0x3E //
#define MPUREG_ACCEL_ZOUT_H 0x3F //
#define MPUREG_ACCEL_ZOUT_L 0x40 //
#define MPUREG_TEMP_OUT_H 0x41//
#define MPUREG_TEMP_OUT_L 0x42//
#define MPUREG_GYRO_XOUT_H 0x43 //
#define MPUREG_GYRO_XOUT_L 0x44 //
#define MPUREG_GYRO_YOUT_H 0x45 //
#define MPUREG_GYRO_YOUT_L 0x46 //
#define MPUREG_GYRO_ZOUT_H 0x47 //
#define MPUREG_GYRO_ZOUT_L 0x48 //
#define MPUREG_USER_CTRL 0x6A //
#define MPUREG_PWR_MGMT_1 0x6B //
#define MPUREG_PWR_MGMT_2 0x6C //
#define MPUREG_FIFO_COUNTH 0x72
#define MPUREG_FIFO_COUNTL 0x73
#define MPUREG_FIFO_R_W 0x74
#define MPUREG_PRODUCT_ID 0x0C // Product ID Register
// Configuration bits MPU 3000 and MPU 6000 (not revised)?
#define BIT_SLEEP 0x40
#define BIT_H_RESET 0x80
#define BITS_CLKSEL 0x07
#define MPU_CLK_SEL_PLLGYROX 0x01
#define MPU_CLK_SEL_PLLGYROZ 0x03
#define MPU_EXT_SYNC_GYROX 0x02
#define BITS_FS_250DPS 0x00
#define BITS_FS_500DPS 0x08
#define BITS_FS_1000DPS 0x10
#define BITS_FS_2000DPS 0x18
#define BITS_FS_MASK 0x18
#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00
#define BITS_DLPF_CFG_188HZ 0x01
#define BITS_DLPF_CFG_98HZ 0x02
#define BITS_DLPF_CFG_42HZ 0x03
#define BITS_DLPF_CFG_20HZ 0x04
#define BITS_DLPF_CFG_10HZ 0x05
#define BITS_DLPF_CFG_5HZ 0x06
#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
#define BITS_DLPF_CFG_MASK 0x07
#define BIT_INT_ANYRD_2CLEAR 0x10
#define BIT_RAW_RDY_EN 0x01
#define BIT_I2C_IF_DIS 0x10
#define BIT_INT_STATUS_DATA 0x01
// Product ID Description for MPU6000
// high 4 bits low 4 bits
// Product Name Product Revision
#define MPU6000ES_REV_C4 0x14 // 0001 0100
#define MPU6000ES_REV_C5 0x15 // 0001 0101
#define MPU6000ES_REV_D6 0x16 // 0001 0110
#define MPU6000ES_REV_D7 0x17 // 0001 0111
#define MPU6000ES_REV_D8 0x18 // 0001 1000
#define MPU6000_REV_C4 0x54 // 0101 0100
#define MPU6000_REV_C5 0x55 // 0101 0101
#define MPU6000_REV_D6 0x56 // 0101 0110
#define MPU6000_REV_D7 0x57 // 0101 0111
#define MPU6000_REV_D8 0x58 // 0101 1000
#define MPU6000_REV_D9 0x59 // 0101 1001
#define MPU6000_REV_D10 0x5A // 0101 1010
static FAR struct mpu6000_dev_s mpu6000_dev;
static ssize_t mpu6000_read(struct file *filp, FAR char *buffer, size_t buflen);
static int mpu6000_ioctl(struct file *filp, int cmd, unsigned long arg);
static const struct file_operations mpu6000_fops = {
.open = 0,
.close = 0,
.read = mpu6000_read,
.write = 0,
.seek = 0,
.ioctl = mpu6000_ioctl,
#ifndef CONFIG_DISABLE_POLL
.poll = 0
#endif
};
struct mpu6000_dev_s
{
struct spi_dev_s *spi;
int spi_id;
uint8_t rate;
struct mpu6000_buffer *buffer;
};
static void mpu6000_write_reg(uint8_t address, uint8_t data);
static uint8_t mpu6000_read_reg(uint8_t address);
static void
mpu6000_write_reg(uint8_t address, uint8_t data)
{
uint8_t cmd[2] = { address | DIR_WRITE, data };
SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, true);
SPI_SNDBLOCK(mpu6000_dev.spi, &cmd, sizeof(cmd));
SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, false);
}
static uint8_t
mpu6000_read_reg(uint8_t address)
{
uint8_t cmd[2] = {address | DIR_READ, 0};
uint8_t data[2];
SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, true);
SPI_EXCHANGE(mpu6000_dev.spi, cmd, data, sizeof(cmd));
SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, false);
return data[1];
}
static int16_t
mpu6000_read_int16(uint8_t address)
{
uint8_t cmd[3] = {address | DIR_READ, 0, 0};
uint8_t data[3];
SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, true);
SPI_EXCHANGE(mpu6000_dev.spi, cmd, data, sizeof(cmd));
SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, false);
return (((int16_t)data[1])<<8) | data[2];
}
static int
mpu6000_set_range(uint8_t range)
{
// /* mask out illegal bit positions */
// uint8_t write_range = range & REG4_RANGE_MASK;
// /* immediately return if user supplied invalid value */
// if (write_range != range) return EINVAL;
// /* set remaining bits to a sane value */
// write_range |= REG4_BDU;
// /* write to device */
// write_reg(ADDR_CTRL_REG4, write_range);
// /* return 0 if register value is now written value, 1 if unchanged */
// return !(read_reg(ADDR_CTRL_REG4) == write_range);
}
static int
mpu6000_set_rate(uint8_t rate)
{
// /* mask out illegal bit positions */
// uint8_t write_rate = rate & REG1_RATE_LP_MASK;
// /* immediately return if user supplied invalid value */
// if (write_rate != rate) return EINVAL;
// /* set remaining bits to a sane value */
// write_rate |= REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
// /* write to device */
// write_reg(ADDR_CTRL_REG1, write_rate);
// /* return 0 if register value is now written value, 1 if unchanged */
// return !(read_reg(ADDR_CTRL_REG1) == write_rate);
}
static int
mpu6000_read_fifo(int16_t *data)
{
// struct { /* status register and data as read back from the device */
// uint8_t cmd;
// uint8_t temp;
// uint8_t status;
// int16_t x;
// int16_t y;
// int16_t z;
// } __attribute__((packed)) report;
//
// report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
//
// /* exchange the report structure with the device */
// SPI_LOCK(mpu6000_dev.spi, true);
//
// SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, true);
//
// read_reg(ADDR_WHO_AM_I);
//
// SPI_EXCHANGE(mpu6000_dev.spi, &report, &report, sizeof(report));
// SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, false);
//
// SPI_LOCK(mpu6000_dev.spi, false);
//
//
//
//
// Device has MSB first at lower address (big endian)
struct { /* status register and data as read back from the device */
// uint8_t cmd;
// uint8_t int_status;
int16_t xacc;
int16_t yacc;
int16_t zacc;
int16_t temp;
int16_t rollspeed;
int16_t pitchspeed;
int16_t yawspeed;
} report;
uint8_t cmd[sizeof(report)];
cmd[0] = MPUREG_ACCEL_XOUT_H | DIR_READ; // was addr_incr
SPI_LOCK(mpu6000_dev.spi, true);
SPI_SELECT(mpu6000_dev.spi, PX4_SPIDEV_MPU, true);
report.xacc = mpu6000_read_int16(MPUREG_ACCEL_XOUT_H);
report.yacc = mpu6000_read_int16(MPUREG_ACCEL_YOUT_H);
report.zacc = mpu6000_read_int16(MPUREG_ACCEL_ZOUT_H);
report.rollspeed = mpu6000_read_int16(MPUREG_GYRO_XOUT_H);
report.pitchspeed = mpu6000_read_int16(MPUREG_GYRO_YOUT_H);
report.yawspeed = mpu6000_read_int16(MPUREG_GYRO_ZOUT_H);
SPI_SELECT(mpu6000_dev.spi, PX4_SPIDEV_MPU, false);
SPI_LOCK(mpu6000_dev.spi, false);
data[0] = report.xacc;
data[1] = report.yacc;
data[2] = report.zacc;
data[3] = report.rollspeed;
data[4] = report.pitchspeed;
data[5] = report.yawspeed;
return 1;//(report.int_status & 0x01);
}
static ssize_t
mpu6000_read(struct file *filp, char *buffer, size_t buflen)
{
/* if the buffer is large enough, and data are available, return success */
if (buflen >= 12) {
if (mpu6000_read_fifo((int16_t *)buffer))
return 12;
/* no data */
return 0;
}
/* buffer too small */
errno = ENOSPC;
return ERROR;
}
static int
mpu6000_ioctl(struct file *filp, int cmd, unsigned long arg)
{
int result = ERROR;
switch (cmd) {
case MPU6000_SETRATE:
if ((arg & 0x00/* XXX REG MASK MISSING */) == arg) {
SPI_LOCK(mpu6000_dev.spi, true);
mpu6000_set_rate(arg);
SPI_LOCK(mpu6000_dev.spi, false);
result = 0;
mpu6000_dev.rate = arg;
}
break;
case MPU6000_SETRANGE:
if ((arg & 0x00/* XXX REG MASK MISSING */) == arg) {
SPI_LOCK(mpu6000_dev.spi, true);
mpu6000_set_range(arg);
SPI_LOCK(mpu6000_dev.spi, false);
result = 0;
}
break;
case MPU6000_SETBUFFER:
mpu6000_dev.buffer = (struct mpu6000_buffer *)arg;
result = 0;
break;
}
if (result)
errno = EINVAL;
return result;
}
int
mpu6000_attach(struct spi_dev_s *spi, int spi_id)
{
int result = ERROR;
mpu6000_dev.spi = spi;
mpu6000_dev.spi_id = spi_id;
SPI_LOCK(mpu6000_dev.spi, true);
// Set sensor-specific SPI mode
SPI_SETFREQUENCY(mpu6000_dev.spi, 10000000); // 500 KHz
SPI_SETBITS(mpu6000_dev.spi, 8);
// Either mode 1 or mode 3
SPI_SETMODE(mpu6000_dev.spi, SPIDEV_MODE3);
// Chip reset
mpu6000_write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
up_udelay(10000);
// Wake up device and select GyroZ clock (better performance)
mpu6000_write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
up_udelay(1000);
// Disable I2C bus (recommended on datasheet)
mpu6000_write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
up_udelay(1000);
// SAMPLE RATE
mpu6000_write_reg(MPUREG_SMPLRT_DIV,0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
usleep(1000);
// FS & DLPF FS=2000¼/s, DLPF = 98Hz (low pass filter)
mpu6000_write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
usleep(1000);
mpu6000_write_reg(MPUREG_GYRO_CONFIG,BITS_FS_2000DPS); // Gyro scale 2000¼/s
usleep(1000);
uint8_t _product_id = mpu6000_read_reg(MPUREG_PRODUCT_ID);
printf("MPU-6000 product id: %d\n", (int)_product_id);
if ((_product_id == MPU6000ES_REV_C4) || (_product_id == MPU6000ES_REV_C5) ||
(_product_id == MPU6000_REV_C4) || (_product_id == MPU6000_REV_C5)){
// Accel scale 8g (4096 LSB/g)
// Rev C has different scaling than rev D
mpu6000_write_reg(MPUREG_ACCEL_CONFIG,1<<3);
} else {
// Accel scale 8g (4096 LSB/g)
mpu6000_write_reg(MPUREG_ACCEL_CONFIG,2<<3);
}
usleep(1000);
// INT CFG => Interrupt on Data Ready
mpu6000_write_reg(MPUREG_INT_ENABLE,BIT_RAW_RDY_EN); // INT: Raw data ready
usleep(1000);
mpu6000_write_reg(MPUREG_INT_PIN_CFG,BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
usleep(1000);
// Oscillator set
// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
usleep(1000);
/* revert back to normal bus mode */
SPI_SETFREQUENCY(mpu6000_dev.spi, 10000000);
SPI_SETBITS(mpu6000_dev.spi, 8);
SPI_SETMODE(mpu6000_dev.spi, SPIDEV_MODE3);
/* verify that the device is attached and functioning */
if ((_product_id == MPU6000ES_REV_C4) || (_product_id == MPU6000ES_REV_C5) ||
(_product_id == MPU6000_REV_C4) || (_product_id == MPU6000_REV_C5) ||
(_product_id == MPU6000_REV_D7) || (_product_id == MPU6000_REV_D8) ||
(_product_id == MPU6000_REV_D9) || (_product_id == MPU6000_REV_D10)){
/* make ourselves available */
register_driver("/dev/mpu6000", &mpu6000_fops, 0666, NULL);
result = OK;
} else {
errno = EIO;
}
SPI_LOCK(mpu6000_dev.spi, false);
SPI_LOCK(mpu6000_dev.spi, false);
return result;
}
nuttx/configs/px4fmu/src/up_cpuload.c
+1
View File
@@ -45,6 +45,7 @@
#include <debug.h> #include <debug.h>
#include <sys/time.h> #include <sys/time.h>
#include <sched.h>
#include <arch/board/board.h> #include <arch/board/board.h>
#include <arch/board/up_hrt.h> #include <arch/board/up_hrt.h>
nuttx/configs/px4fmu/src/up_nsh.c
+14 -81
View File
@@ -1,5 +1,5 @@
/**************************************************************************** /****************************************************************************
* config/stm3210e_eval/src/up_nsh.c * config/px4fmu/src/up_nsh.c
* arch/arm/src/board/up_nsh.c * arch/arm/src/board/up_nsh.c
* *
* Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
@@ -63,11 +63,8 @@
#include <arch/board/board.h> #include <arch/board/board.h>
#include <arch/board/drv_bma180.h> #include <arch/board/drv_bma180.h>
#include <arch/board/drv_l3gd20.h> #include <arch/board/drv_l3gd20.h>
#include <arch/board/drv_hmc5883l.h>
#include <arch/board/drv_mpu6000.h>
#include <arch/board/drv_ms5611.h>
#include <arch/board/drv_eeprom.h>
#include <arch/board/drv_led.h> #include <arch/board/drv_led.h>
#include <arch/board/drv_eeprom.h>
/**************************************************************************** /****************************************************************************
* Pre-Processor Definitions * Pre-Processor Definitions
@@ -95,20 +92,6 @@
* Protected Functions * Protected Functions
****************************************************************************/ ****************************************************************************/
/****************************************************************************
* Name: multiport_setup
*
* Description:
* Perform setup of the PX4FMU's multi function ports
*
****************************************************************************/
int multiport_setup(void)
{
int result = OK;
return result;
}
/**************************************************************************** /****************************************************************************
* Public Functions * Public Functions
****************************************************************************/ ****************************************************************************/
@@ -221,7 +204,7 @@ int nsh_archinitialize(void)
up_udelay(1000); up_udelay(1000);
} }
if (gyro_fail) message("[boot] FAILED to attach L3GD20 gyro\r\n"); if (!gyro_fail) message("[boot] Found L3GD20 gyro\n");
int acc_attempts = 0; int acc_attempts = 0;
int acc_fail = 0; int acc_fail = 0;
@@ -234,13 +217,13 @@ int nsh_archinitialize(void)
up_udelay(1000); up_udelay(1000);
} }
if (acc_fail) message("[boot] FAILED to attach BMA180 accelerometer\r\n"); if (!acc_fail) message("[boot] Found BMA180 accelerometer\n");
/* initialize I2C2 bus */ /* initialize I2C2 bus */
i2c2 = up_i2cinitialize(2); i2c2 = up_i2cinitialize(2);
if (!i2c2) { if (!i2c2) {
message("[boot] FAILED to initialize I2C bus 2\r\n"); message("[boot] FAILED to initialize I2C bus 2\n");
up_ledon(LED_AMBER); up_ledon(LED_AMBER);
return -ENODEV; return -ENODEV;
} }
@@ -251,7 +234,7 @@ int nsh_archinitialize(void)
i2c3 = up_i2cinitialize(3); i2c3 = up_i2cinitialize(3);
if (!i2c3) { if (!i2c3) {
message("[boot] FAILED to initialize I2C bus 3\r\n"); message("[boot] FAILED to initialize I2C bus 3\n");
up_ledon(LED_AMBER); up_ledon(LED_AMBER);
return -ENODEV; return -ENODEV;
} }
@@ -259,91 +242,41 @@ int nsh_archinitialize(void)
/* set I2C3 speed */ /* set I2C3 speed */
I2C_SETFREQUENCY(i2c3, 400000); I2C_SETFREQUENCY(i2c3, 400000);
int mag_attempts = 0;
int mag_fail = 0;
while (mag_attempts < 5)
{
mag_fail = hmc5883l_attach(i2c2);
mag_attempts++;
if (mag_fail == 0) break;
up_udelay(1000);
}
if (mag_fail) message("[boot] FAILED to attach HMC5883L magnetometer\r\n");
int baro_attempts = 0;
int baro_fail = 0;
while (baro_attempts < 5)
{
baro_fail = ms5611_attach(i2c2);
baro_attempts++;
if (baro_fail == 0) break;
up_udelay(1000);
}
if (baro_fail) message("[boot] FAILED to attach MS5611 baro at addr #1 or #2 (0x76 or 0x77)\r\n");
/* try to attach, don't fail if device is not responding */ /* try to attach, don't fail if device is not responding */
(void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS, (void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS,
FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES, FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES,
FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES, FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES,
FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1); FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1);
#if 0
int eeprom_attempts = 0;
int eeprom_fail;
while (eeprom_attempts < 5)
{
/* try to attach, fail if device does not respond */
eeprom_fail = eeprom_attach(i2c2, FMU_ONBOARD_EEPROM_ADDRESS,
FMU_ONBOARD_EEPROM_TOTAL_SIZE_BYTES,
FMU_ONBOARD_EEPROM_PAGE_SIZE_BYTES,
FMU_ONBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/eeprom", 1);
eeprom_attempts++;
if (eeprom_fail == OK) break;
up_udelay(1000);
}
if (eeprom_fail) message("[boot] FAILED to attach FMU EEPROM\r\n");
#endif
/* Report back sensor status */
if (gyro_fail || mag_fail || baro_fail/* || eeprom_fail*/)
{
up_ledon(LED_AMBER);
}
#if defined(CONFIG_STM32_SPI3) #if defined(CONFIG_STM32_SPI3)
/* Get the SPI port */ /* Get the SPI port */
message("[boot] Initializing SPI port 3\r\n"); message("[boot] Initializing SPI port 3\n");
spi3 = up_spiinitialize(3); spi3 = up_spiinitialize(3);
if (!spi3) if (!spi3)
{ {
message("[boot] FAILED to initialize SPI port 3\r\n"); message("[boot] FAILED to initialize SPI port 3\n");
up_ledon(LED_AMBER); up_ledon(LED_AMBER);
return -ENODEV; return -ENODEV;
} }
message("[boot] Successfully initialized SPI port 3\r\n"); message("[boot] Successfully initialized SPI port 3\n");
/* Now bind the SPI interface to the MMCSD driver */ /* Now bind the SPI interface to the MMCSD driver */
result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3); result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
if (result != OK) if (result != OK)
{ {
message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\r\n"); message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
up_ledon(LED_AMBER); up_ledon(LED_AMBER);
return -ENODEV; return -ENODEV;
} }
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\r\n"); message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
#endif /* SPI3 */ #endif /* SPI3 */
/* initialize I2C1 bus */ /* initialize I2C1 bus */
i2c1 = up_i2cinitialize(1); i2c1 = up_i2cinitialize(1);
if (!i2c1) { if (!i2c1) {
message("[boot] FAILED to initialize I2C bus 1\r\n"); message("[boot] FAILED to initialize I2C bus 1\n");
up_ledon(LED_AMBER); up_ledon(LED_AMBER);
return -ENODEV; return -ENODEV;
} }
@@ -355,7 +288,7 @@ int nsh_archinitialize(void)
/* Get board information if available */ /* Get board information if available */
/* Initialize the user GPIOs */ /* Initialize the user GPIOs */
px4fmu_gpio_init(); px4fmu_gpio_init();
#ifdef CONFIG_ADC #ifdef CONFIG_ADC
@@ -367,7 +300,7 @@ int nsh_archinitialize(void)
if (adc_state != OK) if (adc_state != OK)
{ {
/* Give up */ /* Give up */
message("[boot] FAILED adc_devinit: %d\r\n", adc_state); message("[boot] FAILED adc_devinit: %d\n", adc_state);
return -ENODEV; return -ENODEV;
} }
} }