diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/gimbal/gimbal.cpp | 83 | ||||
-rw-r--r-- | src/modules/commander/commander_params.c | 5 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 9 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 5 | ||||
-rw-r--r-- | src/systemcmds/tests/test_ppm_loopback.c | 19 |
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); |