Added position extensions

lispBM/README.md

@@ -102,50 +102,6 @@ Get the age of the last PPM update in seconds. Can be used to determine if there
 
 ---
 
-#### get-encoder
-
-| Platforms | Firmware |
-|---|---|
-| ESC | 6.00+ |
-
-```clj
-(get-encoder)
-```
-
-Get angle from selected encoder in degrees.
-
----
-
-#### set-encoder
-
-| Platforms | Firmware |
-|---|---|
-| ESC | 6.05+ |
-
-```clj
-(set-encoder degrees)
-```
-
-Set the encoder position in degrees. This command only has an effect in the ABI and custom encoder modes. In ABI mode the encoder position is updated and the index is set to found and in custom encoder more the encoder position is updated (unless a native library provides custom encoder support).
-
-When using an ABI-encoder this is useful if a position can be derived before the index pulse is found.
-
----
-
-#### get-encoder-error-rate
-
-| Platforms | Firmware |
-|---|---|
-| ESC | 6.00+ |
-
-```clj
-(get-encoder-error-rate)
-```
-
-Returns the error rate for the selected encoder, range 0.0 to 1.0. If the selected encoder does not provide any error rate -1.0 is returned. If the selected encoder has multiple error rates the highest one is returned.
-
----
-
 #### set-servo
 
 | Platforms | Firmware |
@@ -1275,6 +1231,154 @@ Get the number of watt hours charged since start.
 
 ---
 
+### Positions
+
+There are several position sources and many ways to interpret them. The following extensions can be used to get most interpretations of most position sources.
+
+---
+
+#### get-encoder
+
+| Platforms | Firmware |
+|---|---|
+| ESC | 6.00+ |
+
+```clj
+(get-encoder)
+```
+
+Get angle from selected encoder in degrees.
+
+---
+
+#### set-encoder
+
+| Platforms | Firmware |
+|---|---|
+| ESC | 6.05+ |
+
+```clj
+(set-encoder degrees)
+```
+
+Set the encoder position in degrees. This command only has an effect in the ABI and custom encoder modes. In ABI mode the encoder position is updated and the index is set to found and in custom encoder more the encoder position is updated (unless a native library provides custom encoder support).
+
+When using an ABI-encoder this is useful if a position can be derived before the index pulse is found.
+
+---
+
+#### get-encoder-error-rate
+
+| Platforms | Firmware |
+|---|---|
+| ESC | 6.00+ |
+
+```clj
+(get-encoder-error-rate)
+```
+
+Returns the error rate for the selected encoder, range 0.0 to 1.0. If the selected encoder does not provide any error rate -1.0 is returned. If the selected encoder has multiple error rates the highest one is returned.
+
+---
+
+#### pos-pid-now
+
+| Platforms | Firmware |
+|---|---|
+| ESC | 6.05+ |
+
+```clj
+(pos-pid-now)
+```
+
+Returns the current position of the PID-controller, including the compensation for the angle division and offset. Unit: Degrees.
+
+---
+
+#### pos-pid-set
+
+| Platforms | Firmware |
+|---|---|
+| ESC | 6.05+ |
+
+```clj
+(pos-pid-set)
+```
+
+Returns the set position of the PID-controller, including the compensation for the angle division and offset. Unit: Degrees.
+
+---
+
+#### pos-pid-error
+
+| Platforms | Firmware |
+|---|---|
+| ESC | 6.05+ |
+
+```clj
+(pos-pid-error)
+```
+
+Returns the difference between the current and the set PID position. Unit: Degrees.
+
+---
+
+#### phase-motor
+
+| Platforms | Firmware |
+|---|---|
+| ESC | 6.05+ |
+
+```clj
+(phase-motor)
+```
+
+Returns the electrical position of the motor that is used for FOC now. Unit: Degrees.
+
+---
+
+#### phase-encoder
+
+| Platforms | Firmware |
+|---|---|
+| ESC | 6.05+ |
+
+```clj
+(phase-encoder)
+```
+
+Returns the encoder position mapped to the electrical position of the motor. Unit: Degrees.
+
+---
+
+#### phase-observer
+
+| Platforms | Firmware |
+|---|---|
+| ESC | 6.05+ |
+
+```clj
+(phase-observer)
+```
+
+Returns FOC observer position. Unit: Degrees.
+
+---
+
+#### observer-error
+
+| Platforms | Firmware |
+|---|---|
+| ESC | 6.05+ |
+
+```clj
+(observer-error)
+```
+
+Returns the difference between the observer position and the encoder position mapped to the electrical position of the motor. Unit: Degrees.
+
+---
+
 ### Setup Values
 
 These commands return the accumulated values from all VESC-based motor controllers on the CAN-bus. Note that the corresponding CAN status messages must be activated for these commands to work.

lispBM/lispif.c

@@ -33,8 +33,8 @@
 #define LISP_MEM_BITMAP_SIZE	LBM_MEMORY_BITMAP_SIZE_16K
 #define GC_STACK_SIZE			160
 #define PRINT_STACK_SIZE		128
-#define EXTENSION_STORAGE_SIZE	250
-#define VARIABLE_STORAGE_SIZE	64
+#define EXTENSION_STORAGE_SIZE	260
+#define VARIABLE_STORAGE_SIZE	50
 
 __attribute__((section(".ram4"))) static lbm_cons_t heap[HEAP_SIZE] __attribute__ ((aligned (8)));
 static uint32_t memory_array[LISP_MEM_SIZE];

lispBM/lispif_vesc_extensions.c

@@ -478,22 +478,6 @@ static lbm_value ext_get_ppm_age(lbm_value *args, lbm_uint argn) {
 	return lbm_enc_float((float)servodec_get_time_since_update() / 1000.0);
 }
 
-static lbm_value ext_get_encoder(lbm_value *args, lbm_uint argn) {
-	(void)args; (void)argn;
-	return lbm_enc_float(encoder_read_deg());
-}
-
-static lbm_value ext_set_encoder(lbm_value *args, lbm_uint argn) {
-	LBM_CHECK_ARGN_NUMBER(1);
-	encoder_set_deg(lbm_dec_as_float(args[0]));
-	return ENC_SYM_TRUE;
-}
-
-static lbm_value ext_get_encoder_error_rate(lbm_value *args, lbm_uint argn) {
-	(void)args; (void)argn;
-	return lbm_enc_float(encoder_get_error_rate());
-}
-
 static lbm_value ext_get_vin(lbm_value *args, lbm_uint argn) {
 	(void)args; (void)argn;
 	return lbm_enc_float(mc_interface_get_input_voltage_filtered());
@@ -1582,6 +1566,59 @@ static lbm_value ext_setup_num_vescs(lbm_value *args, lbm_uint argn) {
 	return lbm_enc_i(mc_interface_get_setup_values().num_vescs);
 }
 
+// Positions
+
+static lbm_value ext_get_encoder(lbm_value *args, lbm_uint argn) {
+	(void)args; (void)argn;
+	return lbm_enc_float(encoder_read_deg());
+}
+
+static lbm_value ext_set_encoder(lbm_value *args, lbm_uint argn) {
+	LBM_CHECK_ARGN_NUMBER(1);
+	encoder_set_deg(lbm_dec_as_float(args[0]));
+	return ENC_SYM_TRUE;
+}
+
+static lbm_value ext_get_encoder_error_rate(lbm_value *args, lbm_uint argn) {
+	(void)args; (void)argn;
+	return lbm_enc_float(encoder_get_error_rate());
+}
+
+static lbm_value ext_pos_pid_now(lbm_value *args, lbm_uint argn) {
+	(void)args; (void)argn;
+	return lbm_enc_float(mc_interface_get_pid_pos_now());
+}
+
+static lbm_value ext_pos_pid_set(lbm_value *args, lbm_uint argn) {
+	(void)args; (void)argn;
+	return lbm_enc_float(mc_interface_get_pid_pos_set());
+}
+
+static lbm_value ext_pos_pid_error(lbm_value *args, lbm_uint argn) {
+	(void)args; (void)argn;
+	return lbm_enc_float(utils_angle_difference(mc_interface_get_pid_pos_set(), mc_interface_get_pid_pos_now()));
+}
+
+static lbm_value ext_phase_motor(lbm_value *args, lbm_uint argn) {
+	(void)args; (void)argn;
+	return lbm_enc_float(mcpwm_foc_get_phase());
+}
+
+static lbm_value ext_phase_encoder(lbm_value *args, lbm_uint argn) {
+	(void)args; (void)argn;
+	return lbm_enc_float(mcpwm_foc_get_phase_encoder());
+}
+
+static lbm_value ext_phase_observer(lbm_value *args, lbm_uint argn) {
+	(void)args; (void)argn;
+	return lbm_enc_float(mcpwm_foc_get_phase_observer());
+}
+
+static lbm_value ext_observer_error(lbm_value *args, lbm_uint argn) {
+	(void)args; (void)argn;
+	return lbm_enc_float(utils_angle_difference(mcpwm_foc_get_phase_observer(), mcpwm_foc_get_phase_encoder()));
+}
+
 // CAN-commands
 
 static lbm_value ext_can_current(lbm_value *args, lbm_uint argn) {
@@ -2966,13 +3003,13 @@ static lbm_value ext_conf_set_pid_offset(lbm_value *args, lbm_uint argn) {
 	}
 
 	if (!lbm_is_number(args[0]) || (argn == 2 && !is_symbol_true_false(args[1]))) {
-		return ENC_SYM_EERROR;
+		return ENC_SYM_TERROR;
 	}
 
 	float angle = lbm_dec_as_float(args[0]);
 	if (angle < -360.0 || angle > 360.0) {
 		lbm_set_error_reason("Invalid angle. Range should be -360 to 360.");
-		return ENC_SYM_EERROR;
+		return ENC_SYM_TERROR;
 	}
 
 	bool store = false;
@@ -3872,9 +3909,6 @@ void lispif_load_vesc_extensions(void) {
 	lbm_add_extension("timeout-reset", ext_reset_timeout);
 	lbm_add_extension("get-ppm", ext_get_ppm);
 	lbm_add_extension("get-ppm-age", ext_get_ppm_age);
-	lbm_add_extension("get-encoder", ext_get_encoder);
-	lbm_add_extension("set-encoder", ext_set_encoder);
-	lbm_add_extension("get-encoder-error-rate", ext_get_encoder_error_rate);
 	lbm_add_extension("set-servo", ext_set_servo);
 	lbm_add_extension("get-vin", ext_get_vin);
 	lbm_add_extension("select-motor", ext_select_motor);
@@ -3960,6 +3994,18 @@ void lispif_load_vesc_extensions(void) {
 	lbm_add_extension("get-ah-chg", ext_get_ah_chg);
 	lbm_add_extension("get-wh-chg", ext_get_wh_chg);
 
+	// Positions
+	lbm_add_extension("get-encoder", ext_get_encoder);
+	lbm_add_extension("set-encoder", ext_set_encoder);
+	lbm_add_extension("get-encoder-error-rate", ext_get_encoder_error_rate);
+	lbm_add_extension("pos-pid-now", ext_pos_pid_now);
+	lbm_add_extension("pos-pid-set", ext_pos_pid_set);
+	lbm_add_extension("pos-pid-error", ext_pos_pid_error);
+	lbm_add_extension("phase-motor", ext_phase_motor);
+	lbm_add_extension("phase-encoder", ext_phase_encoder);
+	lbm_add_extension("phase-observer", ext_phase_observer);
+	lbm_add_extension("observer-error", ext_observer_error);
+
 	// Setup values
 	lbm_add_extension("setup-ah", ext_setup_ah);
 	lbm_add_extension("setup-ah-chg", ext_setup_ah_chg);