ardupilot/Blimp/mode_velocity.cpp

42 lines
1.6 KiB
C++
Raw Normal View History

2021-06-18 01:40:01 -03:00
#include "Blimp.h"
/*
* Init and run calls for velocity flight mode
*/
#include <AP_Vehicle/AP_MultiCopter.h>
2021-06-18 01:40:01 -03:00
// Runs the main velocity controller
void ModeVelocity::run()
{
2022-11-30 01:25:42 -04:00
const float dt = blimp.scheduler.get_last_loop_time_s();
2021-06-18 01:40:01 -03:00
Vector3f target_vel;
target_vel.x = channel_front->get_control_in() / float(RC_SCALE) * g.max_vel_xy;
target_vel.y = channel_right->get_control_in() / float(RC_SCALE) * g.max_vel_xy;
2021-06-22 03:36:11 -03:00
blimp.rotate_BF_to_NE(target_vel.xy());
2021-06-18 01:40:01 -03:00
target_vel.z = channel_down->get_control_in() / float(RC_SCALE) * g.max_vel_z;
float target_vel_yaw = channel_yaw->get_control_in() / float(RC_SCALE) * g.max_vel_yaw;
2022-11-30 01:25:42 -04:00
Vector2f actuator = blimp.pid_vel_xy.update_all(target_vel, blimp.vel_ned_filtd, dt, {0,0,0});
2021-06-22 03:36:11 -03:00
blimp.rotate_NE_to_BF(actuator);
2022-11-30 01:25:42 -04:00
float act_down = blimp.pid_vel_z.update_all(target_vel.z, blimp.vel_ned_filtd.z, dt);
float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw, blimp.vel_yaw_filtd, dt);
2021-06-18 01:40:01 -03:00
if(!(blimp.g.dis_mask & (1<<(2-1)))){
2021-06-22 03:36:11 -03:00
motors->front_out = actuator.x;
}
if(!(blimp.g.dis_mask & (1<<(1-1)))){
motors->right_out = actuator.y;
}
if(!(blimp.g.dis_mask & (1<<(3-1)))){
motors->down_out = act_down;
}
if(!(blimp.g.dis_mask & (1<<(4-1)))){
motors->yaw_out = act_yaw;
2021-06-18 01:40:01 -03:00
}
AP::logger().Write_PSCN(0.0, blimp.pos_ned.x * 100.0, 0.0, 0.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0);
AP::logger().Write_PSCE(0.0, blimp.pos_ned.y * 100.0, 0.0, 0.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0);
AP::logger().Write_PSCD(0.0, -blimp.pos_ned.z * 100.0, 0.0, 0.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0);
2021-06-18 01:40:01 -03:00
}