Refloat: LCM now supports "lights_off_when_lifted"

Signed-off-by: Dado Mista <[email protected]>
surfdado · Mar 29, 2024 · b1aa07b · b1aa07b
1 parent c7bf3eb
commit b1aa07b
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 4 deletions.
diff --git 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 = true;
 }
 
 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,11 @@ 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 +96,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
@@ -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,11 @@ 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
@@ -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: {