Skip to content

Commit 4ab4695

Browse files
committed
update protocol for fw 2.4.0
1 parent 7b2aeee commit 4ab4695

26 files changed

Lines changed: 284 additions & 260 deletions

include/tinymovr/nvm.hpp

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
/*
2+
* This file was automatically generated using Avlos.
3+
* https://github.com/tinymovr/avlos
4+
*
5+
* Any changes to this file will be overwritten when
6+
* content is regenerated.
7+
*/
8+
9+
#pragma once
10+
11+
#include <helpers.hpp>
12+
13+
class Nvm_ : Node
14+
{
15+
public:
16+
17+
Nvm_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
18+
Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
19+
uint8_t get_num_slots(void);
20+
uint8_t get_current_slot(void);
21+
uint32_t get_write_count(void);
22+
23+
};

include/tinymovr/tinymovr.hpp

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99
#pragma once
1010

1111
#include <helpers.hpp>
12+
#include <nvm.hpp>
1213
#include <scheduler.hpp>
1314
#include <controller.hpp>
1415
#include <comms.hpp>
@@ -18,7 +19,7 @@
1819
#include <homing.hpp>
1920
#include <watchdog.hpp>
2021

21-
static uint32_t avlos_proto_hash = 641680925;
22+
static uint32_t avlos_proto_hash = 3999954334;
2223

2324
enum errors_flags
2425
{
@@ -160,6 +161,7 @@ class Tinymovr : Node
160161

161162
Tinymovr(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
162163
Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
164+
, nvm(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
163165
, scheduler(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
164166
, controller(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
165167
, comms(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
@@ -181,6 +183,7 @@ class Tinymovr : Node
181183
uint8_t get_warnings(void);
182184
void save_config();
183185
void erase_config();
186+
Nvm_ nvm;
184187
void reset();
185188
void enter_dfu();
186189
uint32_t get_config_size(void);

src/tinymovr/can.cpp

Lines changed: 9 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -11,8 +11,8 @@
1111
uint32_t Can_::get_rate(void)
1212
{
1313
uint32_t value = 0;
14-
this->send(45, this->_data, 0, true);
15-
if (this->recv(45, this->_data, &(this->_dlc), this->delay_us_value))
14+
this->send(48, this->_data, 0, true);
15+
if (this->recv(48, this->_data, &(this->_dlc), this->delay_us_value))
1616
{
1717
read_le(&value, this->_data);
1818
}
@@ -22,14 +22,14 @@ uint32_t Can_::get_rate(void)
2222
void Can_::set_rate(uint32_t value)
2323
{
2424
write_le(value, this->_data);
25-
this->send(45, this->_data, sizeof(uint32_t), false);
25+
this->send(48, this->_data, sizeof(uint32_t), false);
2626
}
2727

2828
uint32_t Can_::get_id(void)
2929
{
3030
uint32_t value = 0;
31-
this->send(46, this->_data, 0, true);
32-
if (this->recv(46, this->_data, &(this->_dlc), this->delay_us_value))
31+
this->send(49, this->_data, 0, true);
32+
if (this->recv(49, this->_data, &(this->_dlc), this->delay_us_value))
3333
{
3434
read_le(&value, this->_data);
3535
}
@@ -39,14 +39,14 @@ uint32_t Can_::get_id(void)
3939
void Can_::set_id(uint32_t value)
4040
{
4141
write_le(value, this->_data);
42-
this->send(46, this->_data, sizeof(uint32_t), false);
42+
this->send(49, this->_data, sizeof(uint32_t), false);
4343
}
4444

4545
bool Can_::get_heartbeat(void)
4646
{
4747
bool value = 0;
48-
this->send(47, this->_data, 0, true);
49-
if (this->recv(47, this->_data, &(this->_dlc), this->delay_us_value))
48+
this->send(50, this->_data, 0, true);
49+
if (this->recv(50, this->_data, &(this->_dlc), this->delay_us_value))
5050
{
5151
read_le(&value, this->_data);
5252
}
@@ -56,8 +56,6 @@ bool Can_::get_heartbeat(void)
5656
void Can_::set_heartbeat(bool value)
5757
{
5858
write_le(value, this->_data);
59-
this->send(47, this->_data, sizeof(bool), false);
59+
this->send(50, this->_data, sizeof(bool), false);
6060
}
6161

62-
63-

src/tinymovr/comms.cpp

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,5 +8,3 @@
88

99
#include <comms.hpp>
1010

11-
12-

src/tinymovr/commutation_sensor.cpp

Lines changed: 12 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -11,8 +11,8 @@
1111
uint8_t Commutation_sensor_::get_connection(void)
1212
{
1313
uint8_t value = 0;
14-
this->send(72, this->_data, 0, true);
15-
if (this->recv(72, this->_data, &(this->_dlc), this->delay_us_value))
14+
this->send(75, this->_data, 0, true);
15+
if (this->recv(75, this->_data, &(this->_dlc), this->delay_us_value))
1616
{
1717
read_le(&value, this->_data);
1818
}
@@ -22,14 +22,14 @@ uint8_t Commutation_sensor_::get_connection(void)
2222
void Commutation_sensor_::set_connection(uint8_t value)
2323
{
2424
write_le(value, this->_data);
25-
this->send(72, this->_data, sizeof(uint8_t), false);
25+
this->send(75, this->_data, sizeof(uint8_t), false);
2626
}
2727

2828
float Commutation_sensor_::get_bandwidth(void)
2929
{
3030
float value = 0;
31-
this->send(73, this->_data, 0, true);
32-
if (this->recv(73, this->_data, &(this->_dlc), this->delay_us_value))
31+
this->send(76, this->_data, 0, true);
32+
if (this->recv(76, this->_data, &(this->_dlc), this->delay_us_value))
3333
{
3434
read_le(&value, this->_data);
3535
}
@@ -39,14 +39,14 @@ float Commutation_sensor_::get_bandwidth(void)
3939
void Commutation_sensor_::set_bandwidth(float value)
4040
{
4141
write_le(value, this->_data);
42-
this->send(73, this->_data, sizeof(float), false);
42+
this->send(76, this->_data, sizeof(float), false);
4343
}
4444

4545
int32_t Commutation_sensor_::get_raw_angle(void)
4646
{
4747
int32_t value = 0;
48-
this->send(74, this->_data, 0, true);
49-
if (this->recv(74, this->_data, &(this->_dlc), this->delay_us_value))
48+
this->send(77, this->_data, 0, true);
49+
if (this->recv(77, this->_data, &(this->_dlc), this->delay_us_value))
5050
{
5151
read_le(&value, this->_data);
5252
}
@@ -56,8 +56,8 @@ int32_t Commutation_sensor_::get_raw_angle(void)
5656
float Commutation_sensor_::get_position_estimate(void)
5757
{
5858
float value = 0;
59-
this->send(75, this->_data, 0, true);
60-
if (this->recv(75, this->_data, &(this->_dlc), this->delay_us_value))
59+
this->send(78, this->_data, 0, true);
60+
if (this->recv(78, this->_data, &(this->_dlc), this->delay_us_value))
6161
{
6262
read_le(&value, this->_data);
6363
}
@@ -67,13 +67,11 @@ float Commutation_sensor_::get_position_estimate(void)
6767
float Commutation_sensor_::get_velocity_estimate(void)
6868
{
6969
float value = 0;
70-
this->send(76, this->_data, 0, true);
71-
if (this->recv(76, this->_data, &(this->_dlc), this->delay_us_value))
70+
this->send(79, this->_data, 0, true);
71+
if (this->recv(79, this->_data, &(this->_dlc), this->delay_us_value))
7272
{
7373
read_le(&value, this->_data);
7474
}
7575
return value;
7676
}
7777

78-
79-

src/tinymovr/controller.cpp

Lines changed: 17 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -11,8 +11,8 @@
1111
uint8_t Controller_::get_state(void)
1212
{
1313
uint8_t value = 0;
14-
this->send(18, this->_data, 0, true);
15-
if (this->recv(18, this->_data, &(this->_dlc), this->delay_us_value))
14+
this->send(21, this->_data, 0, true);
15+
if (this->recv(21, this->_data, &(this->_dlc), this->delay_us_value))
1616
{
1717
read_le(&value, this->_data);
1818
}
@@ -22,14 +22,14 @@ uint8_t Controller_::get_state(void)
2222
void Controller_::set_state(uint8_t value)
2323
{
2424
write_le(value, this->_data);
25-
this->send(18, this->_data, sizeof(uint8_t), false);
25+
this->send(21, this->_data, sizeof(uint8_t), false);
2626
}
2727

2828
uint8_t Controller_::get_mode(void)
2929
{
3030
uint8_t value = 0;
31-
this->send(19, this->_data, 0, true);
32-
if (this->recv(19, this->_data, &(this->_dlc), this->delay_us_value))
31+
this->send(22, this->_data, 0, true);
32+
if (this->recv(22, this->_data, &(this->_dlc), this->delay_us_value))
3333
{
3434
read_le(&value, this->_data);
3535
}
@@ -39,14 +39,14 @@ uint8_t Controller_::get_mode(void)
3939
void Controller_::set_mode(uint8_t value)
4040
{
4141
write_le(value, this->_data);
42-
this->send(19, this->_data, sizeof(uint8_t), false);
42+
this->send(22, this->_data, sizeof(uint8_t), false);
4343
}
4444

4545
uint8_t Controller_::get_warnings(void)
4646
{
4747
uint8_t value = 0;
48-
this->send(20, this->_data, 0, true);
49-
if (this->recv(20, this->_data, &(this->_dlc), this->delay_us_value))
48+
this->send(23, this->_data, 0, true);
49+
if (this->recv(23, this->_data, &(this->_dlc), this->delay_us_value))
5050
{
5151
read_le(&value, this->_data);
5252
}
@@ -56,8 +56,8 @@ uint8_t Controller_::get_warnings(void)
5656
uint8_t Controller_::get_errors(void)
5757
{
5858
uint8_t value = 0;
59-
this->send(21, this->_data, 0, true);
60-
if (this->recv(21, this->_data, &(this->_dlc), this->delay_us_value))
59+
this->send(24, this->_data, 0, true);
60+
if (this->recv(24, this->_data, &(this->_dlc), this->delay_us_value))
6161
{
6262
read_le(&value, this->_data);
6363
}
@@ -67,27 +67,27 @@ uint8_t Controller_::get_errors(void)
6767

6868
void Controller_::calibrate()
6969
{
70-
this->send(39, this->_data, 0, true);
70+
this->send(42, this->_data, 0, true);
7171
}
7272

7373
void Controller_::idle()
7474
{
75-
this->send(40, this->_data, 0, true);
75+
this->send(43, this->_data, 0, true);
7676
}
7777

7878
void Controller_::position_mode()
7979
{
80-
this->send(41, this->_data, 0, true);
80+
this->send(44, this->_data, 0, true);
8181
}
8282

8383
void Controller_::velocity_mode()
8484
{
85-
this->send(42, this->_data, 0, true);
85+
this->send(45, this->_data, 0, true);
8686
}
8787

8888
void Controller_::current_mode()
8989
{
90-
this->send(43, this->_data, 0, true);
90+
this->send(46, this->_data, 0, true);
9191
}
9292

9393
float Controller_::set_pos_vel_setpoints(float pos_setpoint, float vel_setpoint)
@@ -98,14 +98,12 @@ float Controller_::set_pos_vel_setpoints(float pos_setpoint, float vel_setpoint)
9898
write_le(vel_setpoint, this->_data + data_len);
9999
data_len += sizeof(vel_setpoint);
100100

101-
this->send(44, this->_data, data_len, false);
101+
this->send(47, this->_data, data_len, false);
102102
float value = 0;
103103
this->send(17, this->_data, 0, true);
104-
if (this->recv(17, this->_data, &(this->_dlc), this->delay_us_value))
104+
if (this->recv(17, this->_data, &(this->_dlc), this->delay_us_value))
105105
{
106106
read_le(&value, this->_data);
107107
}
108108
return value;
109109
}
110-
111-

0 commit comments

Comments
 (0)