Cansat
/
drone_controller
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: Cansat:master
Branches Tags
Cansat:master
Cansat:cmake_esp32
..
compare: Cansat:cmake_esp32
Branches Tags
Cansat:master
Cansat:cmake_esp32

No commits in common. "master" and "cmake_esp32" have entirely different histories.
master ... cmake_esp3

14 changed files with 26 additions and 115 deletions
Show Stats Expand all files Collapse all files

1
.gitignore vendored
View File

@ -1,2 +1 @@
target/
rust-toolchain.toml

2
CMakeLists.txt
View File

@ -21,7 +21,7 @@ set(RUST_STATIC_LIBRARY "${RUST_TARGET_DIR}/${CARGO_TARGET}/${CARGO_BUILD_TYPE}/
idf_component_register(
SRCS "placeholder.c"
INCLUDE_DIRS ""
INCLUDE_DIRS "" "${RUST_INCLUDE_DIR}"
PRIV_REQUIRES "${RUST_DEPS}"
)
idf_build_get_property(sdkconfig SDKCONFIG)

1
build.rs
View File

@ -16,7 +16,6 @@ fn run_cbindgen(cargo_dir: &Path, target_dir: &Path) {
cbindgen::Builder::new()
.with_crate(cargo_dir)
.with_language(cbindgen::Language::Cxx)
.with_namespace("dcont")
.with_pragma_once(true)
.generate()
.expect("Unable to generate bindings")

2
rust-toolchain.toml Normal file
View File

@ -0,0 +1,2 @@
[toolchain]
channel = "esp"

31
src/ccont.rs
View File

@ -53,25 +53,16 @@ pub extern "C" fn set_input(ptr: *mut StackedController, inp: Input) {
controller.set_input(inp);
}
#[unsafe(no_mangle)]
pub extern "C" fn set_max_linvel(ptr: *mut StackedController, max_linvel: f32) {
pub extern "C" fn set_cur_rot(ptr: *mut StackedController, parts: Vec3C, scalar: f32) {
let controller = unsafe {
assert!(!ptr.is_null());
&mut *ptr
};
controller.set_max_linvel(max_linvel);
}
#[unsafe(no_mangle)]
pub extern "C" fn set_cur_rot(ptr: *mut StackedController, rot: QuatC) {
let controller = unsafe {
assert!(!ptr.is_null());
&mut *ptr
};
controller.set_rotation(rot.into());
let quat: na::UnitQuaternion<f32> =
na::UnitQuaternion::from_quaternion(na::Quaternion::from_parts(scalar, parts.into()));
controller.set_rotation(quat);
}
#[unsafe(no_mangle)]
@ -103,17 +94,3 @@ pub extern "C" fn set_cur_pos(ptr: *mut StackedController, pos: Vec3C) {
controller.set_pos(pos.into());
}
#[unsafe(no_mangle)]
pub extern "C" fn reset_pid_states(ptr: *mut StackedController){
let controller = unsafe {
assert!(!ptr.is_null());
&mut *ptr
};
controller.angvel_rt.module.pid.reset_state();
controller.rotation_rt.module.pid.reset_state();
// controller.linaccel_rt.module.pid.reset_state();
controller.linvel_rt.module.pid.reset_state();
controller.position_rt.module.pid.reset_state();
}

3
src/config.rs
View File

@ -19,14 +19,13 @@ pub struct ControllerStackConfig {
/// Maximum angular rate (rad/s) that joystick input maps to
pub max_rate: f32,
pub max_linvel: f32,
}
#[repr(C)]
pub struct ControllerConfig {
pub stack: ControllerStackConfig,
// For each motor, [Roll, Pitch, Yaw] (Rotation around x,y,z)
// For each motor, [Pitch, Roll, Yaw] (Rotation around x,y,z)
pub motor_map: [[f32; 3]; 4],
pub mass: f32,

8
src/input.rs
View File

@ -13,9 +13,9 @@ pub struct JoystickInput {
#[repr(C)]
#[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)]
pub struct PositionInput {
pub x: f32,
pub y: f32,
pub z: f32,
pub lat: f32,
pub long: f32,
pub alt: f32,
}
#[repr(C)]
@ -50,7 +50,7 @@ pub enum ModeInput {
Rotation,
Acceleration,
Velocity,
Position,
Navigation,
}
#[repr(C)]

44
src/stacked.rs
View File

@ -25,11 +25,10 @@ pub struct DroneState {
}
pub struct StackedController {
pub angvel_rt: ModuleRuntime<AngularRateController>,
pub rotation_rt: ModuleRuntime<RotationController>,
pub rate_rt: ModuleRuntime<AngularRateController>,
pub linaccel_rt: ModuleRuntime<AccelerationController>,
pub linvel_rt: ModuleRuntime<VelocityController>,
pub position_rt: ModuleRuntime<PositionController>,
mixer: MotorMixer,
config: ControllerConfig,
@ -42,17 +41,10 @@ pub struct StackedController {
}
impl StackedController {
pub fn set_max_linvel(&mut self, max_linvel: f32) {
self.position_rt.module.max_linvel = max_linvel;
}
pub fn new(config: ControllerConfig) -> Self {
let min_throttle = 0.0;
let max_throttle = 1.0;
let position_ctrl = PositionController::new(
PidProcessor::new(&config.stack.position_pid),
config.stack.max_linvel,
);
let lin_vel_ctrl = VelocityController::new(PidProcessor::new(&config.stack.linvel_pid));
let lin_accel_ctrl =
AccelerationController::new(max_throttle, min_throttle, config.max_thrust);
@ -62,13 +54,12 @@ impl StackedController {
Self {
linaccel_rt: ModuleRuntime::new(lin_accel_ctrl, config.stack.rotation_pid.frequency),
rotation_rt: ModuleRuntime::new(rotation_ctrl, config.stack.rotation_pid.frequency),
angvel_rt: ModuleRuntime::new(rate_ctrl, config.stack.rate_pid.frequency),
rate_rt: ModuleRuntime::new(rate_ctrl, config.stack.rate_pid.frequency),
linvel_rt: ModuleRuntime::new(lin_vel_ctrl, config.stack.linvel_pid.frequency),
position_rt: ModuleRuntime::new(position_ctrl, config.stack.position_pid.frequency),
mixer: MotorMixer {
motor_map: config.motor_map,
min_throttle: min_throttle,
max_throttle: max_throttle,
min_throttle: 0.0,
max_throttle: 1.0,
mixing_mode: mixer::MotorMixingMode::default(),
},
@ -119,29 +110,7 @@ impl DroneController for StackedController {
let angular_rate_setpoint = if self.input.mode != ModeInput::Acro {
let rotation_setpoint = if self.input.mode != ModeInput::Rotation {
let lin_accel_setpoint = if self.input.mode != ModeInput::Acceleration {
let lin_vel_setpoint: Velocity = if self.input.mode != ModeInput::Velocity {
let position_setpoint: Position = if self.input.mode != ModeInput::Position
{
println!("LAYER NOT IMPLENETED. TAKING 0 AS INPUT;");
Position(na::Vector3::zeros())
} else {
Position(na::vector![
self.input.position.x,
self.input.position.y,
self.input.position.z
])
};
self.position_rt
.update(position_setpoint, &self.drone_state, frame_dt)
} else {
Velocity(na::vector![
self.input.velocity.x,
self.input.velocity.y,
self.input.velocity.z
])
};
self.linvel_rt
.update(lin_vel_setpoint, &self.drone_state, frame_dt)
panic!("Not Implemented")
} else {
Acceleration(na::vector![
self.input.acceleration.x,
@ -155,6 +124,7 @@ impl DroneController for StackedController {
throttle = ret.1 .0;
ret.0
} else {
// println!("Rotation!");
Rotation(na::vector![
self.input.rotation.roll,
self.input.rotation.pitch,
@ -174,7 +144,7 @@ impl DroneController for StackedController {
};
let torque = self
.angvel_rt
.rate_rt
.update(angular_rate_setpoint, &self.drone_state, frame_dt);
self.mixer.mix(throttle, torque.0)

8
src/stacked/mixer.rs
View File

@ -7,7 +7,7 @@ pub enum MotorMixingMode {
}
impl Default for MotorMixingMode {
fn default() -> Self {
Self::ThrottleAuthorityReasonable { min_scale: 0.05 }
Self::ThrottleAuthorityReasonable { min_scale: 0.1 }
}
}
@ -82,9 +82,9 @@ impl MotorMixer {
let delta_dif = max_delta - min_delta;
if delta_dif > throttle_range_len {
scale *= throttle_range_len / delta_dif * 0.98;
max_delta *= throttle_range_len / delta_dif * 0.98;
min_delta *= throttle_range_len / delta_dif * 0.98;
scale *= throttle_range_len / delta_dif * 0.99;
max_delta *= throttle_range_len / delta_dif * 0.99;
min_delta *= throttle_range_len / delta_dif * 0.99;
}
let lim_throttle = throttle.clamp(

9
src/stacked/modules.rs
View File

@ -1,17 +1,14 @@
use crate::DroneState;
use crate::PidConfig;
use nalgebra as na;
use nalgebra::zero;
pub mod acceleration;
pub mod angular_rate;
pub mod position;
pub mod rotation;
pub mod velocity;
pub use acceleration::AccelerationController;
pub use angular_rate::AngularRateController;
pub use position::PositionController;
pub use rotation::RotationController;
pub use velocity::VelocityController;
@ -102,12 +99,6 @@ impl PidProcessor {
}
}
pub fn reset_state(&mut self) {
self.integral = na::Vector3::<f32>::zeros();
self.last_error = na::Vector3::<f32>::zeros();
// self.last_error = 0.0;
}
pub fn update(
&mut self,
target: na::Vector3<f32>,

2
src/stacked/modules/angular_rate.rs
View File

@ -1,7 +1,7 @@
use crate::stacked::modules::*;
pub struct AngularRateController {
pub pid: PidProcessor,
pid: PidProcessor,
}
impl AngularRateController {

26
src/stacked/modules/position.rs
View File

@ -1,26 +0,0 @@
use crate::stacked::modules::*;
pub struct PositionController {
pub pid: PidProcessor,
pub max_linvel: f32,
}
impl PositionController {
pub fn new(pid: PidProcessor, max_linvel: f32) -> Self {
Self { pid, max_linvel }
}
}
impl ControllerModule for PositionController {
type Input = Position;
type Output = Velocity;
fn process(&mut self, input: Position, state: &DroneState, dt: f32) -> Velocity {
let target_rate = input.0;
let output: na::Vector3<f32> = self.pid.update(target_rate, state.position, dt);
let norm = output.norm().clamp(0.0, self.max_linvel);
Velocity(output.normalize() * norm)
}
}

2
src/stacked/modules/rotation.rs
View File

@ -3,7 +3,7 @@ use nalgebra::UnitQuaternion;
use crate::stacked::modules::*;
pub struct RotationController {
pub pid: PidProcessor,
pid: PidProcessor,
}
impl RotationController {
pub fn new(pid: PidProcessor) -> Self {

2
src/stacked/modules/velocity.rs
View File

@ -1,7 +1,7 @@
use crate::stacked::modules::*;
pub struct VelocityController {
pub pid: PidProcessor,
pid: PidProcessor,
}
impl VelocityController {