aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/drivers/gimbal/gimbal.cpp83
-rw-r--r--src/modules/commander/commander_params.c5
-rw-r--r--src/modules/sdlog2/sdlog2.c9
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h5
-rw-r--r--src/systemcmds/tests/test_ppm_loopback.c19
5 files changed, 86 insertions, 35 deletions
diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp
index ccda4c0a5..1e27309d8 100644
--- a/src/drivers/gimbal/gimbal.cpp
+++ b/src/drivers/gimbal/gimbal.cpp
@@ -121,8 +121,12 @@ private:
int _vehicle_command_sub;
int _att_sub;
- bool _attitude_compensation;
+ bool _attitude_compensation_roll;
+ bool _attitude_compensation_pitch;
+ bool _attitude_compensation_yaw;
bool _initialized;
+ bool _control_cmd_set;
+ bool _config_cmd_set;
orb_advert_t _actuator_controls_2_topic;
@@ -130,6 +134,9 @@ private:
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
+ struct vehicle_command_s _control_cmd;
+ struct vehicle_command_s _config_cmd;
+
/**
* Initialise the automatic measurement state machine and start it.
*
@@ -169,7 +176,9 @@ Gimbal::Gimbal() :
CDev("Gimbal", GIMBAL_DEVICE_PATH),
_vehicle_command_sub(-1),
_att_sub(-1),
- _attitude_compensation(true),
+ _attitude_compensation_roll(true),
+ _attitude_compensation_pitch(true),
+ _attitude_compensation_yaw(true),
_initialized(false),
_actuator_controls_2_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "gimbal_read")),
@@ -221,7 +230,9 @@ Gimbal::ioctl(struct file *filp, int cmd, unsigned long arg)
switch (cmd) {
case GIMBALIOCATTCOMPENSATE:
- _attitude_compensation = (arg != 0);
+ _attitude_compensation_roll = (arg != 0);
+ _attitude_compensation_pitch = (arg != 0);
+ _attitude_compensation_yaw = (arg != 0);
return OK;
default:
@@ -286,22 +297,30 @@ Gimbal::cycle()
float yaw = 0.0f;
- if (_attitude_compensation) {
- if (_att_sub < 0) {
- _att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
- }
+ if (_att_sub < 0) {
+ _att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+ }
- vehicle_attitude_s att;
+ vehicle_attitude_s att;
- orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);
+ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);
+ if (_attitude_compensation_roll) {
roll = -att.roll;
+ updated = true;
+ }
+
+ if (_attitude_compensation_pitch) {
pitch = -att.pitch;
- yaw = att.yaw;
+ updated = true;
+ }
+ if (_attitude_compensation_yaw) {
+ yaw = att.yaw;
updated = true;
}
+
struct vehicle_command_s cmd;
bool cmd_updated;
@@ -312,22 +331,47 @@ Gimbal::cycle()
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
- VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)cmd.param7;
- debug("cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", cmd.command, mountMode, (double)cmd.param1, (double)cmd.param2);
+ if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL
+ || cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) {
+
+ _control_cmd = cmd;
+ _control_cmd_set = true;
+
+ } else if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
- if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
+ _config_cmd = cmd;
+ _config_cmd_set = true;
+ }
+
+ }
+
+ if (_config_cmd_set) {
+
+ _config_cmd_set = false;
+
+ _attitude_compensation_roll = (int)_config_cmd.param2 == 1;
+ _attitude_compensation_pitch = (int)_config_cmd.param3 == 1;
+ _attitude_compensation_yaw = (int)_config_cmd.param4 == 1;
+
+ }
+
+ if (_control_cmd_set) {
+
+ VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)_control_cmd.param7;
+ debug("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2);
+
+ if (_control_cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
/* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */
- roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param1;
- pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param2;
- yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param3;
+ roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1;
+ pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2;
+ yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3;
updated = true;
-
}
- if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
- float gimbalDirectionQuat[] = {cmd.param1, cmd.param2, cmd.param3, cmd.param4};
+ if (_control_cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
+ float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4};
math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler();
roll += gimablDirectionEuler(0);
@@ -336,6 +380,7 @@ Gimbal::cycle()
updated = true;
}
+
}
if (updated) {
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 1b0c4258b..94eeb6f71 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -55,6 +55,7 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
* Defines the voltage where a single cell of the battery is considered empty.
*
* @group Battery Calibration
+ * @unit V
*/
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
@@ -64,6 +65,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
* Defines the voltage where a single cell of the battery is considered full.
*
* @group Battery Calibration
+ * @unit V
*/
PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
@@ -74,6 +76,7 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
* to maximum current ratio and assumes linearity.
*
* @group Battery Calibration
+ * @unit V
*/
PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
@@ -83,6 +86,7 @@ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
* Defines the number of cells the attached battery consists of.
*
* @group Battery Calibration
+ * @unit S
*/
PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
@@ -92,6 +96,7 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
* Defines the capacity of the attached battery.
*
* @group Battery Calibration
+ * @unit mA
*/
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 7017a65ad..04bf71c17 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1216,12 +1216,13 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- VEHICLE STATUS --- */
if (status_updated) {
log_msg.msg_type = LOG_STAT_MSG;
- log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
- log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
- log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe;
+ log_msg.body.log_STAT.main_state = buf_status.main_state;
+ log_msg.body.log_STAT.arming_state = buf_status.arming_state;
+ log_msg.body.log_STAT.failsafe = (uint8_t) buf_status.failsafe;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
- log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
+ log_msg.body.log_STAT.battery_warning = buf_status.battery_warning;
log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
+ log_msg.body.log_STAT.load = buf_status.load;
LOGBUFFER_WRITE_AND_COUNT(STAT);
}
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 87b4795bc..060dcf62f 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -171,10 +171,11 @@ struct log_ATTC_s {
struct log_STAT_s {
uint8_t main_state;
uint8_t arming_state;
- uint8_t failsafe_state;
+ uint8_t failsafe;
float battery_remaining;
uint8_t battery_warning;
uint8_t landed;
+ float load;
};
/* --- RC - RC INPUT CHANNELS --- */
@@ -480,7 +481,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
- LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"),
+ LOG_FORMAT(STAT, "BBBfBBf", "MainS,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"),
LOG_FORMAT(VTOL, "f", "Arsp"),
LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c
index a753f45c2..6866531d7 100644
--- a/src/systemcmds/tests/test_ppm_loopback.c
+++ b/src/systemcmds/tests/test_ppm_loopback.c
@@ -105,16 +105,15 @@ int test_ppm_loopback(int argc, char *argv[])
int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400};
- // for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
- // result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]);
-
- // if (result) {
- // (void)close(servo_fd);
- // return ERROR;
- // } else {
- // warnx("channel %d set to %d", i, pwm_values[i]);
- // }
- // }
+ for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
+ result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]);
+ if (result) {
+ (void)close(servo_fd);
+ return ERROR;
+ } else {
+ warnx("channel %d set to %d", i, pwm_values[i]);
+ }
+ }
warnx("servo count: %d", servo_count);
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-25 13:59:50 +0000