diff --git a/refloat/refloat/lcm.c b/refloat/refloat/lcm.c index ae5ad912b..c3659c027 100644 --- a/refloat/refloat/lcm.c +++ b/refloat/refloat/lcm.c @@ -31,6 +31,7 @@ void lcm_init(LcmData *lcm, CfgHwLeds *hw_cfg) { lcm->status_brightness = 0; lcm->name[0] = '\0'; lcm->payload_size = 0; + lcm->lights_off_when_lifted = 1; } void lcm_configure(LcmData *lcm, const CfgLeds *cfg) { @@ -48,6 +49,7 @@ void lcm_configure(LcmData *lcm, const CfgLeds *cfg) { } lcm->brightness_idle = cfg->front.brightness * 100; } + lcm->lights_off_when_lifted = cfg->lights_off_when_lifted; } void lcm_poll_request(LcmData *lcm, uint8_t *buffer, size_t len) { @@ -68,7 +70,8 @@ void lcm_poll_request(LcmData *lcm, uint8_t *buffer, size_t len) { } void lcm_poll_response( - LcmData *lcm, const State *state, FootpadSensorState fs_state, const MotorData *motor + LcmData *lcm, const State *state, FootpadSensorState fs_state, + const MotorData *motor, const float pitch ) { if (!lcm->enabled) { return; @@ -90,7 +93,13 @@ void lcm_poll_response( buffer[ind++] = send_state; buffer[ind++] = VESC_IF->mc_get_fault(); - buffer[ind++] = fminf(100, fabsf(motor->duty_cycle * 100)); + if (state->state == STATE_RUNNING) { + buffer[ind++] = fminf(100, fabsf(motor->duty_cycle * 100)); + } else { + // pitch is a value between -180 and +180, so abs(pitch) fits into uint8 + buffer[ind++] = lcm->lights_off_when_lifted ? fabsf(pitch) : 0; + } + buffer_append_float16(buffer, motor->erpm, 1e0, &ind); buffer_append_float16(buffer, VESC_IF->mc_get_tot_current_in(), 1e0, &ind); buffer_append_float16(buffer, VESC_IF->mc_get_input_voltage_filtered(), 1e1, &ind); diff --git a/refloat/refloat/lcm.h b/refloat/refloat/lcm.h index a860d6111..0cd773014 100644 --- a/refloat/refloat/lcm.h +++ b/refloat/refloat/lcm.h @@ -41,6 +41,7 @@ typedef struct { uint8_t brightness; uint8_t brightness_idle; uint8_t status_brightness; + bool lights_off_when_lifted; char name[MAX_LCM_NAME_LENGTH]; uint8_t payload[MAX_LCM_PAYLOAD_LENGTH]; @@ -60,7 +61,7 @@ void lcm_poll_request(LcmData *lcm, uint8_t *buffer, size_t len); * Response to the LCM poll request to get data from the package. */ void lcm_poll_response( - LcmData *lcm, const State *state, FootpadSensorState fs_state, const MotorData *motor + LcmData *lcm, const State *state, FootpadSensorState fs_state, const MotorData *motor, const float pitch ); /** diff --git a/refloat/refloat/main.c b/refloat/refloat/main.c index 16029f229..e683f357e 100644 --- a/refloat/refloat/main.c +++ b/refloat/refloat/main.c @@ -2549,7 +2549,7 @@ static void on_command_received(unsigned char *buffer, unsigned int len) { } case COMMAND_LCM_POLL: { lcm_poll_request(&d->lcm, &buffer[2], len - 2); - lcm_poll_response(&d->lcm, &d->state, d->footpad_sensor.state, &d->motor); + lcm_poll_response(&d->lcm, &d->state, d->footpad_sensor.state, &d->motor, d->pitch); return; } case COMMAND_LCM_LIGHT_INFO: {