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

Cleaned up / simplified position control, attacking pos control implementation next

Browse Source
This commit is contained in:
Lorenz Meier
2012-10-04 14:50:34 +02:00
parent 2a06b66845
commit 607e902b88
5 changed files with 288 additions and 208 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/examples/px4_deamon_app/px4_deamon_app.c
+4 -1
View File
@@ -97,7 +97,6 @@ int px4_deamon_app_main(int argc, char *argv[])
4096, 4096,
px4_deamon_thread_main, px4_deamon_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL); (argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0); exit(0);
} }
@@ -123,6 +122,8 @@ int px4_deamon_thread_main(int argc, char *argv[]) {
printf("[deamon] starting\n"); printf("[deamon] starting\n");
thread_running = true;
while (!thread_should_exit) { while (!thread_should_exit) {
printf("Hello Deamon!\n"); printf("Hello Deamon!\n");
sleep(10); sleep(10);
@@ -130,5 +131,7 @@ int px4_deamon_thread_main(int argc, char *argv[]) {
printf("[deamon] exiting.\n"); printf("[deamon] exiting.\n");
thread_running = false;
return 0; return 0;
} }
apps/multirotor_pos_control/multirotor_pos_control.c
+94 -18
View File
@@ -49,24 +49,92 @@
#include <time.h> #include <time.h>
#include <sys/prctl.h> #include <sys/prctl.h>
#include <arch/board/up_hrt.h> #include <arch/board/up_hrt.h>
#include "ardrone_control.h"
#include "attitude_control.h"
#include "rate_control.h"
#include "ardrone_motor_control.h"
#include "position_control.h"
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
#include <systemlib/systemlib.h>
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
__EXPORT int multirotor_pos_control_main(int argc, char *argv[]); __EXPORT int multirotor_pos_control_main(int argc, char *argv[]);
static bool thread_should_exit; /**
static bool thread_running = false; * Mainloop of position controller.
static int mpc_task; */
static int multirotor_pos_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_spawn().
*/
int multirotor_pos_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("multirotor pos control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("multirotor pos control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 60,
4096,
multirotor_pos_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tmultirotor pos control app is running\n");
} else {
printf("\tmultirotor pos control app not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
static int static int
mpc_thread_main(int argc, char *argv[]) multirotor_pos_control_thread_main(int argc, char *argv[])
{ {
/* welcome user */ /* welcome user */
printf("[multirotor pos control] Control started, taking over position control\n"); printf("[multirotor pos control] Control started, taking over position control\n");
@@ -91,6 +159,8 @@ mpc_thread_main(int argc, char *argv[])
/* publish attitude setpoint */ /* publish attitude setpoint */
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
thread_running = true;
while (1) { while (1) {
/* get a local copy of the vehicle state */ /* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state); orb_copy(ORB_ID(vehicle_status), state_sub, &state);
@@ -103,11 +173,19 @@ mpc_thread_main(int argc, char *argv[])
/* get a local copy of local position setpoint */ /* get a local copy of local position setpoint */
orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
if (state.state_machine == SYSTEM_STATE_AUTO) { // if (state.state_machine == SYSTEM_STATE_AUTO) {
position_control(&state, &manual, &att, &local_pos, &local_pos_sp, &att_sp);
// XXX IMPLEMENT POSITION CONTROL HERE
att_sp.roll_body = 0.1f;
att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.4f;
att_sp.timestamp = hrt_absolute_time();
/* publish new attitude setpoint */ /* publish new attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
} else if (state.state_machine == SYSTEM_STATE_STABILIZE) { // } else if (state.state_machine == SYSTEM_STATE_STABILIZED) {
/* set setpoint to current position */ /* set setpoint to current position */
// XXX select pos reset channel on remote // XXX select pos reset channel on remote
/* reset setpoint to current position (position hold) */ /* reset setpoint to current position (position hold) */
@@ -117,19 +195,17 @@ mpc_thread_main(int argc, char *argv[])
// local_pos_sp.z = local_pos.z; // local_pos_sp.z = local_pos.z;
// local_pos_sp.yaw = att.yaw; // local_pos_sp.yaw = att.yaw;
// } // }
} // }
/* run at approximately 50 Hz */ /* run at approximately 50 Hz */
usleep(20000); usleep(20000);
counter++;
} }
/* close uarts */ printf("[multirotor pos control] ending now...\n");
close(ardrone_write);
ar_multiplexing_deinit(gpios); thread_running = false;
printf("[multirotor pos control] ending now...\r\n");
fflush(stdout); fflush(stdout);
return 0; return 0;
} }
apps/multirotor_pos_control/position_control.c
+225 -225
View File
@@ -1,235 +1,235 @@
/**************************************************************************** // /****************************************************************************
* // *
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. // * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch> // * Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Laurens Mackay <mackayl@student.ethz.ch> // * @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch> // * @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch> // * @author Martin Rutschmann <rutmarti@student.ethz.ch>
* // *
* Redistribution and use in source and binary forms, with or without // * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions // * modification, are permitted provided that the following conditions
* are met: // * are met:
* // *
* 1. Redistributions of source code must retain the above copyright // * 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer. // * notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright // * 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in // * notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the // * the documentation and/or other materials provided with the
* distribution. // * distribution.
* 3. Neither the name PX4 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 // * used to endorse or promote products derived from this software
* without specific prior written permission. // * without specific prior written permission.
* // *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS // * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED // * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE. // * POSSIBILITY OF SUCH DAMAGE.
* // *
****************************************************************************/ // ****************************************************************************/
/** // /**
* @file multirotor_position_control.c // * @file multirotor_position_control.c
* Implementation of the position control for a multirotor VTOL // * Implementation of the position control for a multirotor VTOL
*/ // */
#include <stdio.h> // #include <stdio.h>
#include <stdlib.h> // #include <stdlib.h>
#include <stdio.h> // #include <stdio.h>
#include <stdint.h> // #include <stdint.h>
#include <math.h> // #include <math.h>
#include <stdbool.h> // #include <stdbool.h>
#include <float.h> // #include <float.h>
#include <systemlib/pid/pid.h> // #include <systemlib/pid/pid.h>
#include "multirotor_position_control.h" // #include "multirotor_position_control.h"
void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, // void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, // const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp) // const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
{
static PID_t distance_controller;
static int read_ret;
static global_data_position_t position_estimated;
static uint16_t counter;
static bool initialized;
static uint16_t pm_counter;
static float lat_next;
static float lon_next;
static float pitch_current;
static float thrust_total;
if (initialized == false) {
pid_init(&distance_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
PID_MODE_DERIVATIV_CALC, 150);//150
// pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
thrust_total = 0.0f;
/* Position initialization */
/* Wait for new position estimate */
do {
read_ret = read_lock_position(&position_estimated);
} while (read_ret != 0);
lat_next = position_estimated.lat;
lon_next = position_estimated.lon;
/* attitude initialization */
global_data_lock(&global_data_attitude->access_conf);
pitch_current = global_data_attitude->pitch;
global_data_unlock(&global_data_attitude->access_conf);
initialized = true;
}
/* load new parameters with 10Hz */
if (counter % 50 == 0) {
if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
/* check whether new parameters are available */
if (global_data_parameter_storage->counter > pm_counter) {
pid_set_parameters(&distance_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
//
// pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
pm_counter = global_data_parameter_storage->counter;
printf("Position controller changed pid parameters\n");
}
}
global_data_unlock(&global_data_parameter_storage->access_conf);
}
/* Wait for new position estimate */
do {
read_ret = read_lock_position(&position_estimated);
} while (read_ret != 0);
/* Get next waypoint */ //TODO: add local copy
if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
lat_next = global_data_position_setpoint->x;
lon_next = global_data_position_setpoint->y;
global_data_unlock(&global_data_position_setpoint->access_conf);
}
/* Get distance to waypoint */
float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
// if(counter % 5 == 0)
// printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
/* Get bearing to waypoint (direction on earth surface to next waypoint) */
float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
if (counter % 5 == 0)
printf("bearing: %.4f\n", bearing);
/* Calculate speed in direction of bearing (needed for controller) */
float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy);
// if(counter % 5 == 0)
// printf("speed_norm: %.4f\n", speed_norm);
float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
/* Control Thrust in bearing direction */
float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
// if(counter % 5 == 0)
// printf("horizontal thrust: %.4f\n", horizontal_thrust);
/* Get total thrust (from remote for now) */
if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum?
global_data_unlock(&global_data_rc_channels->access_conf);
}
const float max_gas = 500.0f;
thrust_total *= max_gas / 20000.0f; //TODO: check this
thrust_total += max_gas / 2.0f;
if (horizontal_thrust > thrust_total) {
horizontal_thrust = thrust_total;
} else if (horizontal_thrust < -thrust_total) {
horizontal_thrust = -thrust_total;
}
//TODO: maybe we want to add a speed controller later...
/* Calclulate thrust in east and north direction */
float thrust_north = cosf(bearing) * horizontal_thrust;
float thrust_east = sinf(bearing) * horizontal_thrust;
if (counter % 10 == 0) {
printf("thrust north: %.4f\n", thrust_north);
printf("thrust east: %.4f\n", thrust_east);
fflush(stdout);
}
/* Get current attitude */
if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
pitch_current = global_data_attitude->pitch;
global_data_unlock(&global_data_attitude->access_conf);
}
/* Get desired pitch & roll */
float pitch_desired = 0.0f;
float roll_desired = 0.0f;
if (thrust_total != 0) {
float pitch_fraction = -thrust_north / thrust_total;
float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
if (roll_fraction < -1) {
roll_fraction = -1;
} else if (roll_fraction > 1) {
roll_fraction = 1;
}
// if(counter % 5 == 0)
// { // {
// printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction); // static PID_t distance_controller;
// static int read_ret;
// static global_data_position_t position_estimated;
// static uint16_t counter;
// static bool initialized;
// static uint16_t pm_counter;
// static float lat_next;
// static float lon_next;
// static float pitch_current;
// static float thrust_total;
// if (initialized == false) {
// pid_init(&distance_controller,
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
// PID_MODE_DERIVATIV_CALC, 150);//150
// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
// thrust_total = 0.0f;
// /* Position initialization */
// /* Wait for new position estimate */
// do {
// read_ret = read_lock_position(&position_estimated);
// } while (read_ret != 0);
// lat_next = position_estimated.lat;
// lon_next = position_estimated.lon;
// /* attitude initialization */
// global_data_lock(&global_data_attitude->access_conf);
// pitch_current = global_data_attitude->pitch;
// global_data_unlock(&global_data_attitude->access_conf);
// initialized = true;
// }
// /* load new parameters with 10Hz */
// if (counter % 50 == 0) {
// if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
// /* check whether new parameters are available */
// if (global_data_parameter_storage->counter > pm_counter) {
// pid_set_parameters(&distance_controller,
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
// //
// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
// pm_counter = global_data_parameter_storage->counter;
// printf("Position controller changed pid parameters\n");
// }
// }
// global_data_unlock(&global_data_parameter_storage->access_conf);
// }
// /* Wait for new position estimate */
// do {
// read_ret = read_lock_position(&position_estimated);
// } while (read_ret != 0);
// /* Get next waypoint */ //TODO: add local copy
// if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
// lat_next = global_data_position_setpoint->x;
// lon_next = global_data_position_setpoint->y;
// global_data_unlock(&global_data_position_setpoint->access_conf);
// }
// /* Get distance to waypoint */
// float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
// // if(counter % 5 == 0)
// // printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
// /* Get bearing to waypoint (direction on earth surface to next waypoint) */
// float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
// if (counter % 5 == 0)
// printf("bearing: %.4f\n", bearing);
// /* Calculate speed in direction of bearing (needed for controller) */
// float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy);
// // if(counter % 5 == 0)
// // printf("speed_norm: %.4f\n", speed_norm);
// float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
// /* Control Thrust in bearing direction */
// float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
// CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
// // if(counter % 5 == 0)
// // printf("horizontal thrust: %.4f\n", horizontal_thrust);
// /* Get total thrust (from remote for now) */
// if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
// thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum?
// global_data_unlock(&global_data_rc_channels->access_conf);
// }
// const float max_gas = 500.0f;
// thrust_total *= max_gas / 20000.0f; //TODO: check this
// thrust_total += max_gas / 2.0f;
// if (horizontal_thrust > thrust_total) {
// horizontal_thrust = thrust_total;
// } else if (horizontal_thrust < -thrust_total) {
// horizontal_thrust = -thrust_total;
// }
// //TODO: maybe we want to add a speed controller later...
// /* Calclulate thrust in east and north direction */
// float thrust_north = cosf(bearing) * horizontal_thrust;
// float thrust_east = sinf(bearing) * horizontal_thrust;
// if (counter % 10 == 0) {
// printf("thrust north: %.4f\n", thrust_north);
// printf("thrust east: %.4f\n", thrust_east);
// fflush(stdout); // fflush(stdout);
// } // }
pitch_desired = asinf(pitch_fraction); // /* Get current attitude */
roll_desired = asinf(roll_fraction); // if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
} // pitch_current = global_data_attitude->pitch;
// global_data_unlock(&global_data_attitude->access_conf);
// }
att_sp.roll = roll_desired; // /* Get desired pitch & roll */
att_sp.pitch = pitch_desired; // float pitch_desired = 0.0f;
// float roll_desired = 0.0f;
counter++; // if (thrust_total != 0) {
} // float pitch_fraction = -thrust_north / thrust_total;
// float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
// if (roll_fraction < -1) {
// roll_fraction = -1;
// } else if (roll_fraction > 1) {
// roll_fraction = 1;
// }
// // if(counter % 5 == 0)
// // {
// // printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
// // fflush(stdout);
// // }
// pitch_desired = asinf(pitch_fraction);
// roll_desired = asinf(roll_fraction);
// }
// att_sp.roll = roll_desired;
// att_sp.pitch = pitch_desired;
// counter++;
// }
apps/multirotor_pos_control/position_control.h
+6 -6
View File
@@ -40,11 +40,11 @@
* Definition of the position control for a multirotor VTOL * Definition of the position control for a multirotor VTOL
*/ */
#ifndef POSITION_CONTROL_H_ // #ifndef POSITION_CONTROL_H_
#define POSITION_CONTROL_H_ // #define POSITION_CONTROL_H_
void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, // void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, // const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp); // const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp);
#endif /* POSITION_CONTROL_H_ */ // #endif /* POSITION_CONTROL_H_ */
nuttx/configs/px4fmu/nsh/appconfig
+1
View File
@@ -74,6 +74,7 @@ CONFIGURED_APPS += sdlog
CONFIGURED_APPS += sensors CONFIGURED_APPS += sensors
CONFIGURED_APPS += ardrone_interface CONFIGURED_APPS += ardrone_interface
CONFIGURED_APPS += multirotor_att_control CONFIGURED_APPS += multirotor_att_control
CONFIGURED_APPS += multirotor_pos_control
CONFIGURED_APPS += px4/attitude_estimator_bm CONFIGURED_APPS += px4/attitude_estimator_bm
CONFIGURED_APPS += fixedwing_control CONFIGURED_APPS += fixedwing_control
CONFIGURED_APPS += position_estimator CONFIGURED_APPS += position_estimator